
#include <string>
#include <vector>


#include <alglib/src/ap.h>
#include <alglib/src/optimization.h>
#include <alglib/src/interpolation.h>

#include <base/utils/utils.h>
#include <base/Svar/Scommand.h>
#include <base/osa/osa++.h>
#include <hardware/Gps/GPS.h>
#include <uav/utils_mavlink.h>

#include "ui_controls.h"
#include "utils_qt.h"
#include "GCS_MainWindow.h"

using namespace std;
using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_diagAbout::UI_diagAbout(QWidget *parent, int autoClose) : QDialog(parent)
{
    m_autoClose = autoClose;
    m_openTS = tm_getTimeStamp();

    setupUI();

    if( m_autoClose )
        startTimer(100);
}

UI_diagAbout::~UI_diagAbout()
{

}

void UI_diagAbout::setupUI(void)
{
    this->setObjectName(QString::fromUtf8("dlgAbout"));

    QSize s(891, 325);
    this->resize(s);
    this->setMaximumSize(s);
    this->setMinimumSize(s);
    if( m_autoClose )   this->setWindowTitle("FastGCS");
    else                this->setWindowTitle("About");

    labLogo = new QLabel(this);
    labLogo->setObjectName(QString::fromUtf8("labLogo"));
    labLogo->setGeometry(QRect(8, 8, 875, 265));
    labLogo->setPixmap(QPixmap(QString::fromUtf8(":/images/images/pis_about.png")));

    btnOK = new QPushButton(this);
    btnOK->setObjectName(QString::fromUtf8("btnOK"));
    btnOK->setText("&OK");
    btnOK->setGeometry(QRect(318, 278, 369, 35));

    connect(btnOK, SIGNAL(clicked(bool)), this, SLOT(btnOK_clicked(bool)));
}

void UI_diagAbout::btnOK_clicked(bool checked)
{
    this->close();
}

void UI_diagAbout::mousePressEvent(QMouseEvent *ev)
{
    QDesktopServices::openUrl(QUrl("http://www.pilotintelligence.com", QUrl::TolerantMode));
}

void UI_diagAbout::timerEvent(QTimerEvent *event)
{
    if( m_autoClose ) {
        double tsNow = tm_getTimeStamp();
        if( tsNow - m_openTS > svar.GetDouble("FastGCS.splashScreen.showTime", 5.0) ) {
            close();
        }
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_diagUAVSelect::UI_diagUAVSelect(QWidget *parent, UAS_Manager *u) :
    QDialog(parent), m_uasManager(u)
{
    setupUI();
}

UI_diagUAVSelect::~UI_diagUAVSelect()
{

}

void UI_diagUAVSelect::setupUI(void)
{
    this->setObjectName(QString::fromUtf8("UAV Selection"));
    this->resize(258, 119);

    labUAV = new QLabel(this);
    labUAV->setObjectName(QString::fromUtf8("labUAV"));
    labUAV->setText("UAV:");
    labUAV->setGeometry(QRect(20, 30, 57, 15));
    cbUAV = new QComboBox(this);
    cbUAV->setObjectName(QString::fromUtf8("cbUAV"));
    cbUAV->setGeometry(QRect(50, 30, 171, 21));

    if( m_uasManager != NULL ) {
        UAS_Array *ua = m_uasManager->get_uas();

        for(int i=0; i<ua->size(); i++) {
            cbUAV->addItem(QString("%1").arg(ua->at(i)->ID));
        }
    }

    buttonBox = new QDialogButtonBox(this);
    buttonBox->setObjectName(QString::fromUtf8("buttonBox"));
    buttonBox->setGeometry(QRect(30, 60, 201, 32));
    buttonBox->setOrientation(Qt::Horizontal);
    buttonBox->setStandardButtons(QDialogButtonBox::Cancel|QDialogButtonBox::Ok);


    QObject::connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
    QObject::connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
}

int UI_diagUAVSelect::getSelectedUAV(void)
{
    return cbUAV->currentText().toInt();
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_paramterList::UI_paramterList(QWidget *parent) : QWidget(parent)
{
    // set default parameters
    clCL1 = QColor(0x00, 0x00, 0xFF);
    clCL2 = QColor(0x00, 0x00, 0x00);
    clB1  = QColor(0xFF, 0xFF, 0xFF);
    clB2  = QColor(0xE8, 0xE8, 0xE8);
    clB3  = QColor(0xFF, 0x00, 0x00);

    fontSize  = 10;
    rowHeight = 24;

    m_updateCell = 0;
    m_tableFilled = 0;

    m_arrParam = NULL;
    m_sFilter = "";

    this->setMinimumSize(600, 200);

    // set table widget
    m_tblParam = new QTableWidget(this);
    m_tblParam->setObjectName(QString::fromUtf8("tableParameters"));
    m_tblParam->verticalHeader()->hide();

#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
    m_tblParam->horizontalHeader()->setResizeMode(QHeaderView::Interactive);
#else
    m_tblParam->horizontalHeader()->setSectionResizeMode(QHeaderView::Interactive);
#endif

    m_tblParam->horizontalHeader()->setDefaultAlignment(Qt::AlignLeft);
    m_tblParam->setColumnCount(4);

    m_tblParam->setColumnWidth(0, 60);
    m_tblParam->setColumnWidth(1, 200);
    m_tblParam->setColumnWidth(2, 100);
    m_tblParam->setColumnWidth(3, 400);

    QStringList slHeader;
    slHeader.append("Index");
    slHeader.append("ID/Name");
    slHeader.append("Value");
    slHeader.append("Description");
    m_tblParam->setHorizontalHeaderLabels(slHeader);

    connect(m_tblParam, SIGNAL(cellChanged(int,int)),
            this, SLOT(act_cellChanged(int,int)));

    // set vbox layout
    QVBoxLayout *verticalLayout = new QVBoxLayout(this);
    verticalLayout->setContentsMargins(0, 0, 0, 0);
    verticalLayout->addWidget(m_tblParam);
}

UI_paramterList::~UI_paramterList()
{
    m_updateCell = 1;
    delete m_tblParam;
}

int UI_paramterList::set_paramArray(AP_ParamArray *arrParam)
{
    AP_ParamIndexMap *pim = NULL;
    QStringList sl;
    int i, n;

    // check input arguments
    if( arrParam == NULL ) return -1;

    // set parameter array & map
    m_arrParam = arrParam;
    pim = m_arrParam->get_paramIndexMap();
    n = pim->size();    // FIXME: parameter index must be continuous

    if( m_sFilter.size() > 0 ) {
        // get filtered items
        for(i=0; i<n; i++) {
            AP_ParamItem *pi = pim->at(i);
            QString s = pi->id;

            int p = s.indexOf(m_sFilter, 0, Qt::CaseInsensitive);
            if( p >= 0 ) sl.push_back(s);
        }
    } else {
        // get all items
        for(i=0; i<n; i++) {
            AP_ParamItem *pi = pim->at(i);
            QString s = pi->id;

            sl.push_back(s);
        }
    }

    // show parameter list
    set_paramArray_(sl);

    return 0;
}

int UI_paramterList::set_filter(QString sf)
{
    m_sFilter = sf;
    set_paramArray(m_arrParam);

    return 0;
}

int UI_paramterList::set_paramArray_(QStringList &slID)
{
    // clear old contents & set filted items
    m_tblParam->clearContents();

    m_updateCell = 1;

    m_tblParam->setRowCount(slID.size());

    for(int i=0; i<slID.size(); i++) {
        AP_ParamItem *pi = m_arrParam->get(slID[i].toStdString());
        if( pi != NULL ) set_tableItem(i, pi);
    }

    // set flags
    m_updateCell = 0;
    m_tableFilled = 1;

    // resize column width
    resize_column_width();
}


int UI_paramterList::resize_column_width(void)
{
    int w = this->width();
    int cw[4], w4;

    for(int i=0; i<4; i++)
        cw[i] = m_tblParam->columnWidth(i);

    w4 = w - cw[0] - cw[1] - cw[2] - 25;
    if( w4 < 100 ) w4 = 100;

    m_tblParam->setColumnWidth(3, w4);
}

int UI_paramterList::set_tableItem(int ri, int ci, QString s)
{
    if( m_tblParam->item(ri, ci) != NULL ) {
        m_tblParam->item(ri, ci)->setText(s);
    } else {
        QTableWidgetItem* item = new QTableWidgetItem();
        item->setText(s);

        item->setTextColor(clCL2);
        if( ri % 2 == 0 ) item->setBackgroundColor(clB1);
        else              item->setBackgroundColor(clB2);

        item->setFont(QFont("", fontSize));
        m_tblParam->setItem(ri, ci, item);
    }

    return 0;
}

int UI_paramterList::set_tableItem(int ri, AP_ParamItem *pi)
{
    // set index
    set_tableItem(ri, 0, QString("%1").arg(pi->index));
    m_tblParam->item(ri, 0)->setFlags(Qt::ItemIsSelectable);

    // set ID/name
    set_tableItem(ri, 1, pi->id);
    m_tblParam->item(ri, 1)->setFlags(Qt::ItemIsSelectable);

    // set value
    QString s;
    if( ! (pi->type == MAV_PARAM_TYPE_REAL32 || pi->type == MAV_PARAM_TYPE_REAL64 ) )
        s = QString("%1").arg(pi->toInt32());
    else
        s = QString("%1").arg(pi->value);
    set_tableItem(ri, 2, s);

    if( pi->modified ) {
        m_tblParam->item(ri, 2)->setTextColor(clCL1);

        m_tblParam->item(ri, 2)->setBackgroundColor(clB3);
    } else {
        m_tblParam->item(ri, 2)->setTextColor(clCL2);

        if( ri % 2 == 0 ) m_tblParam->item(ri, 2)->setBackgroundColor(clB1);
        else              m_tblParam->item(ri, 2)->setBackgroundColor(clB2);
    }

    // set description
    set_tableItem(ri, 3, "");
    m_tblParam->item(ri, 3)->setFlags(Qt::ItemIsSelectable);

    // set row height
    m_tblParam->setRowHeight(ri, rowHeight);

    return 0;
}


void UI_paramterList::act_cellChanged(int row, int column)
{
    if( !m_updateCell ) {
        m_updateCell = 1;

        // get parameter index
        int id = m_tblParam->item(row, 0)->text().toInt();

        // set modify flag & update value
        AP_ParamItem *pi = m_arrParam->get(id);

        pi->modified = 1;
        if( pi->type == MAV_PARAM_TYPE_REAL32 || pi->type == MAV_PARAM_TYPE_REAL64 )
            pi->value = m_tblParam->item(row, column)->text().toFloat();
        else
            pi->value = m_tblParam->item(row, column)->text().toInt();

        // set font/backgrond color
        if( column == 2 ) {
            m_tblParam->item(row, column)->setTextColor(clCL1);
            m_tblParam->item(row, column)->setBackgroundColor(clB3);
        }

        m_updateCell = 0;
    }
}

void UI_paramterList::resizeEvent(QResizeEvent *event)
{
    if( m_tableFilled ) {
        resize_column_width();
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_AP_paramControl::UI_AP_paramControl(QWidget *parent) : QWidget(parent)
{
    setupUI();

    m_activeUAS = -1;

    //startTimer(200);
}

UI_AP_paramControl::~UI_AP_paramControl()
{

}

void UI_AP_paramControl::setupUI(void)
{
    // button pannel
    QWidget *btnPannel = new QWidget(this);
    btnPannel->setMinimumSize(QSize(100, 40));

    QLabel *labUAS = new QLabel(btnPannel);
    labUAS->setGeometry(QRect(8, 12, 33, 16));
    labUAS->setText("UAS:");
    m_cbUAS = new QComboBox(btnPannel);
    m_cbUAS->setGeometry(QRect(40, 5, 79, 27));

    m_btnRead = new QPushButton(btnPannel);
    m_btnRead->setObjectName(QString::fromUtf8("btnRead"));
    m_btnRead->setGeometry(QRect(132, 6, 85, 27));
    m_btnRead->setText("Read");
    m_btnRead->setToolTip("Read from device");
    m_btnWrite = new QPushButton(btnPannel);
    m_btnWrite->setObjectName(QString::fromUtf8("btnWrite"));
    m_btnWrite->setGeometry(QRect(224, 6, 85, 27));
    m_btnWrite->setText("Write");
    m_btnWrite->setToolTip("Write to device");

    QLabel *labFilter = new QLabel(btnPannel);
    labFilter->setObjectName(QString::fromUtf8("labFilter"));
    labFilter->setGeometry(QRect(334, 10, 37, 16));
    labFilter->setText("Filter:");
    m_edtFilter = new QLineEdit(btnPannel);
    m_edtFilter->setObjectName(QString::fromUtf8("edtFilter"));
    m_edtFilter->setGeometry(QRect(370, 6, 113, 25));
    m_edtFilter->setFocus();

    m_btnLoad = new QPushButton(btnPannel);
    m_btnLoad->setObjectName(QString::fromUtf8("btnLoad"));
    m_btnLoad->setGeometry(QRect(514, 6, 85, 27));
    m_btnLoad->setText("Load");
    m_btnLoad->setToolTip("Load from file");
    m_btnSave = new QPushButton(btnPannel);
    m_btnSave->setObjectName(QString::fromUtf8("btnSave"));
    m_btnSave->setGeometry(QRect(602, 6, 85, 27));
    m_btnSave->setText("Save");
    m_btnSave->setToolTip("Save to file");

    // parameter list widget
    m_tblParam = new UI_paramterList(this);

    // vbox layout
    QVBoxLayout *verticalLayout = new QVBoxLayout(this);

    verticalLayout->addWidget(btnPannel);
    verticalLayout->addWidget(m_tblParam);

    setMinimumSize(750, 500);

    // connect signals
    connect(m_cbUAS, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(cbUAS_currentIndexChanged(QString)));

    connect(m_btnSave, SIGNAL(clicked(bool)),
            this, SLOT(btnSave_clicked(bool)));
    connect(m_btnLoad, SIGNAL(clicked(bool)),
            this, SLOT(btnLoad_clicked(bool)));
    connect(m_btnRead, SIGNAL(clicked(bool)),
            this, SLOT(btnRead_clicked(bool)));
    connect(m_btnWrite, SIGNAL(clicked(bool)),
            this, SLOT(btnWrite_clicked(bool)));

    connect(m_edtFilter, SIGNAL(editingFinished()),
            this, SLOT(edtFilter_editingFinished()));
}

int UI_AP_paramControl::set_UAS_Manager(UAS_Manager *uasManager)
{
    // set UAS manager
    m_uasManager = uasManager;

    // get available UASs
    UAS_Array *arrUAS = m_uasManager->get_uas();

    UAS_Array::iterator it;
    UAS_Base *u;

    m_cbUAS->clear();

    // add UAS and load corresponding parameters
    for(it=arrUAS->begin(); it!=arrUAS->end(); it++) {
        u = *it;

        if( u->uasType == UAS_TYPE_TELEM ) continue;

        m_cbUAS->addItem(QString("%1").arg(u->ID));

        //printf("uas [%3d] parameter loaded = %d\n", u->ID,
        //       u->m_paramArray.get_loaded());

        if( !u->m_paramArray.isLoaded() ) {
            u->requireParameters();

            UI_diagParamProgress diag;
            diag.setUAS(u);
            diag.exec();
        }
    }

    // set current index to first item
    m_cbUAS->setCurrentIndex(0);
}


void UI_AP_paramControl::timerEvent(QTimerEvent *event)
{

}

void UI_AP_paramControl::cbUAS_currentIndexChanged(const QString &text)
{
    if( text.size() <= 0 ) return;

    m_activeUAS = text.toInt();

    UAS_Base *u = m_uasManager->get_uas(m_activeUAS);
    if( u != NULL ) {
        m_tblParam->set_paramArray( &(u->m_paramArray) );
    }
}

void UI_AP_paramControl::btnRead_clicked(bool checked)
{
    // read parameter from device
    if( m_activeUAS >= 0 ) {
        UAS_Base *u = m_uasManager->get_uas(m_activeUAS);
        if( u == NULL ) return;

        u->requireParameters(1);

        UI_diagParamProgress diag;
        diag.setUAS(u);
        diag.exec();

        m_tblParam->set_paramArray(&(u->m_paramArray));
    }
}

void UI_AP_paramControl::btnWrite_clicked(bool checked)
{
    // write parameter to device
    if( m_activeUAS >= 0 ) {
        UAS_Base *u = m_uasManager->get_uas(m_activeUAS);
        if( u == NULL ) return;

        if( !u->m_paramArray.isLoaded() ) {
            dbg_pe("Parameters of UAS[%d] are not loaded!", u->ID);
            return;
        } else {
            u->updateParameters();

            UI_diagParamProgress diag;
            diag.setUAS(u, 1);
            diag.exec();

            m_tblParam->set_paramArray(&(u->m_paramArray));
        }
    }
}

void UI_AP_paramControl::btnSave_clicked(bool checked)
{
    string dp = svar.GetString("FastGCS.DataPath", "./Data/FastGCS");
    string path = path_join(dp, "apm_param");

    QString fname;

    // get save filename
    fname = QFileDialog::getSaveFileName(this, tr("Parameter file"),
                                         QString::fromStdString(path),
                                         tr("Parameter Files (*.pa)"));

    if( fname.size() < 1 ) return;
    if( !fname.endsWith(".pa", Qt::CaseInsensitive) ) {
        fname = fname + ".pa";
    }

    // save to file
    if( m_activeUAS >= 0 ) {
        UAS_Base *u = m_uasManager->get_uas(m_activeUAS);
        if( u == NULL ) return;

        if( u->m_paramArray.isLoaded() ) {
            u->m_paramArray.save(fname.toStdString());
        }
    }
}

void UI_AP_paramControl::btnLoad_clicked(bool checked)
{
    // get save filename
    string dp = svar.GetString("FastGCS.DataPath", "./Data/FastGCS");
    string path = path_join(dp, "apm_param");

    QString fname;

    fname = QFileDialog::getOpenFileName(this, tr("Parameter file"),
                                         QString::fromStdString(path),
                                         tr("Parameter Files (*.pa)"));
    if( fname.size() < 1 ) return;

    // load parameters
    AP_ParamArray pa;
    if( 0 != pa.load(fname.toStdString()) ) return;

    // update to control
    if( m_activeUAS >= 0 ) {
        UAS_Base *u = m_uasManager->get_uas(m_activeUAS);
        if( u == NULL ) return;

        u->m_paramArray.set(pa);
        u->m_paramArray.setLoaded(1);
        m_tblParam->set_paramArray(&(u->m_paramArray));
    }
}

void UI_AP_paramControl::edtFilter_editingFinished(void)
{
    m_tblParam->set_filter(m_edtFilter->text());
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_waypointDelegate::UI_waypointDelegate(QObject *parent)
    : QItemDelegate(parent)
{
    mavlink_nameList nl;
    mavlink_mav_cmdList(nl);

    for(int i=0; i<nl.size(); i++) m_wpTypeList.append(QString::fromStdString(nl[i]));
}

QWidget *UI_waypointDelegate::createEditor(QWidget *parent,
                                       const QStyleOptionViewItem &opt,
                                       const QModelIndex &index) const
{
    int colIdx = index.column();
    int rowIdx = index.row();

    QStringList slHome;
    slHome.append("Home");

    if( colIdx == 0 ) {
        // index
        return NULL;
    } else if( colIdx == 1 || colIdx == 2) {
        // altitude & heading
        QLineEdit *editor = new QLineEdit(parent);

        return editor;
    } else if ( colIdx == 3 ) {
        // type
        QComboBox *editor = new QComboBox(parent);

        if( rowIdx == 0 )
            editor->addItems(slHome);
        else
            editor->addItems(m_wpTypeList);

        editor->setEditable(false);

        return editor;
    }

    return NULL;
}

void UI_waypointDelegate::setEditorData(QWidget *editor,
                                    const QModelIndex &index) const
{
    int colIdx = index.column();

    if( colIdx == 0 ) {
        // index
        return;
    } else if( colIdx == 1 || colIdx == 2 ) {
        // altitude & heading
        QLineEdit *lineEdit = static_cast<QLineEdit*>(editor);
        lineEdit->setText(index.model()->data(index, Qt::EditRole).toString());
    } else if( colIdx == 3 ) {
        // type
        QComboBox *cb = static_cast<QComboBox*>(editor);
        int cbIdx = cb->findText(index.model()->data(index, Qt::EditRole).toString());
        cb->setCurrentIndex(cbIdx);
    }
}

void UI_waypointDelegate::setModelData(QWidget *editor, QAbstractItemModel *model,
                                   const QModelIndex &index) const
{
    int colIdx = index.column();

    if( colIdx == 0 ) {
        // index
        return;
    } else if( colIdx == 1 || colIdx == 2 ) {
        // altitude & heading
        QLineEdit *lineEdit = static_cast<QLineEdit*>(editor);
        model->setData(index, lineEdit->text(), Qt::EditRole);
    } else if( colIdx == 3 ) {
        // type
        QComboBox *cb = static_cast<QComboBox*>(editor);
        model->setData(index, cb->currentText(), Qt::EditRole);
    }
}

void UI_waypointDelegate::updateEditorGeometry(QWidget *editor,
                                           const QStyleOptionViewItem &option, const QModelIndex &/* index */) const
{
    editor->setGeometry(option.rect);
}



UI_waypointEdit::UI_waypointEdit(QWidget *parent) : QWidget(parent)
{
    this->setWindowTitle("Waypoints Edit");

    m_wpMap  = NULL;
    m_wpIdx  = 0;

    clCL1 = QColor(0x00, 0x00, 0xFF);
    clCL2 = QColor(0x00, 0x00, 0x00);
    clB1  = QColor(0xFF, 0xFF, 0xFF);
    clB2  = QColor(0xE8, 0xE8, 0xE8);

    fontSize  = 10;
    rowHeight = 24;

    m_bHeightAltitude = 1;
    m_homeAltitude = 440.0;


    setupUi();
}

UI_waypointEdit::~UI_waypointEdit()
{

}


void UI_waypointEdit::setupUi(void)
{
    if (this->objectName().isEmpty())
        this->setObjectName(QString::fromUtf8("WaypointEdit"));
    this->setMinimumSize(400, 300);

    //////////////////////////////////////////////////////////
    /// waypoint edit table
    //////////////////////////////////////////////////////////
    wpModel = new QStandardItemModel;
    wpDelegate = new UI_waypointDelegate;
    wpTable = new QTableView(this);
    wpTable->setObjectName("wpTable");
    wpTable->setModel(wpModel);
    wpTable->setItemDelegate(wpDelegate);

    initWaypointList();


    //////////////////////////////////////////////////////////
    /// control pannel
    //////////////////////////////////////////////////////////
    QWidget *controlPanel = new QWidget(this);
    controlPanel->setMinimumSize(400, 80);

    // all waypoint checkbox
    cbAllWaypoints = new QCheckBox(controlPanel);
    cbAllWaypoints->setObjectName(QString::fromUtf8("cbAllWaypoints"));
    cbAllWaypoints->setGeometry(QRect(10, 10, 200, 20));
    cbAllWaypoints->setText("All Waypoints");
    connect(cbAllWaypoints, SIGNAL(clicked(bool)),
            this, SLOT(act_cbAllWaypoints_clicked(bool)));

    // height / altitude checkbox
    cbHeightAltitude = new QCheckBox(controlPanel);
    cbHeightAltitude->setObjectName(QString::fromUtf8("cbHeightAltitude"));
    cbHeightAltitude->setGeometry(QRect(10, 50, 200, 20));
    cbHeightAltitude->setText("Height / Altitude");
    connect(cbHeightAltitude, SIGNAL(clicked(bool)),
            this, SLOT(act_cbHeightAltitude_clicked(bool)));

    // button for setting all height
    QPushButton* btnSetAllHeight = new QPushButton(controlPanel);
    btnSetAllHeight->setObjectName("btnSetAllHeight");
    btnSetAllHeight->setGeometry(QRect(240, 10, 120, 30));
    btnSetAllHeight->setText("Set All Height");
    connect(btnSetAllHeight, SIGNAL(clicked()),
            this, SLOT(act_btnSetAllHeight()));


    //////////////////////////////////////////////////////////
    /// layout
    //////////////////////////////////////////////////////////
    verticalLayout = new QVBoxLayout(this);
    verticalLayout->setObjectName(QString::fromUtf8("verticalLayout"));

    verticalLayout->addWidget(wpTable);
    verticalLayout->addWidget(controlPanel);

    QMetaObject::connectSlotsByName(this);
}

void UI_waypointEdit::initWaypointList(void)
{
    wpTable->verticalHeader()->hide();
    wpTable->horizontalHeader()->setDefaultAlignment(Qt::AlignLeft);

#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
    wpTable->horizontalHeader()->setResizeMode(QHeaderView::Interactive);
#else
    wpTable->horizontalHeader()->setSectionResizeMode(QHeaderView::Interactive);
#endif

    wpModel->setColumnCount(4);

    QStringList slHeader;
    slHeader.append("Index");
    slHeader.append("Altitude");
    slHeader.append("Heading");
    slHeader.append("Type");
    wpModel->setHorizontalHeaderLabels(slHeader);
}

int UI_waypointEdit::setWaypoints(int idx, QMap<int, mapcontrol::WayPointItem*> *wpMap,
                               int heightAltitude)
{
    m_wpIdx  = idx;
    m_wpMap  = wpMap;
    m_bHeightAltitude = heightAltitude;

    // set height/altitude checkbox
    if( heightAltitude ) {
        cbHeightAltitude->setCheckState(Qt::Checked);
    } else {
        cbHeightAltitude->setCheckState(Qt::Unchecked);
    }

    // if edit all waypoints then disable checkbox of all waypoints
    if( m_wpIdx < 0 ) cbAllWaypoints->setDisabled(true);

    // set table items
    setWaypoints_(idx, wpMap, heightAltitude);

    return 0;
}

int UI_waypointEdit::setWaypoints_(int idx,
                                QMap<int, mapcontrol::WayPointItem*> *wpMap,
                                int heightAltitude)
{
    // clear old contents
    wpModel->clear();

    // init table view
    initWaypointList();

    // set horizontal header
    if( heightAltitude ) {
        wpModel->horizontalHeaderItem(1)->setText("Height");
    } else {
        wpModel->horizontalHeaderItem(1)->setText("Altitude");
    }

    // insert items
    double h;

    if( idx >= 0 ) {
        cbAllWaypoints->setCheckState(Qt::Unchecked);
        wpModel->setRowCount(1);

        mapcontrol::WayPointItem *item;
        item = wpMap->value(idx);

        if( heightAltitude ) h = item->Height();
        else                 h = item->Height() + m_homeAltitude;

        setTableItem(0, 0, QString("%1").arg(item->Number()));
        setTableItem(0, 1, QString("%1").arg(h));
        setTableItem(0, 2, QString("%1").arg(item->Heading()));
        setTableItem(0, 3, item->WaypointType());

        wpModel->item(0, 0)->setFlags(Qt::ItemIsSelectable);
        wpModel->item(0, 2)->setToolTip("0:North, 90:East, 180:South, 270:West");
    } else {
        cbAllWaypoints->setCheckState(Qt::Checked);

        QList<int> ids = wpMap->keys();
        int        ri = 0;

        wpModel->setRowCount(ids.size());

        foreach(int i, ids) {
            mapcontrol::WayPointItem *item;
            item = wpMap->value(i);

            if( heightAltitude ) h = item->Height();
            else                 h = item->Height() + m_homeAltitude;

            setTableItem(ri, 0, QString("%1").arg(item->Number()));
            setTableItem(ri, 1, QString("%1").arg(h));
            setTableItem(ri, 2, QString("%1").arg(item->Heading()));
            setTableItem(ri, 3, item->WaypointType());

            wpModel->item(ri, 0)->setFlags(Qt::ItemIsSelectable);
            wpModel->item(ri, 2)->setToolTip("0:North, 90:East, 180:South, 270:West");

            ri++;
        }
    }

    return 0;
}

int UI_waypointEdit::setTableItem(int ri, int ci, QString s)
{
    if( wpModel->item(ri, ci) != NULL ) {
        wpModel->item(ri, ci)->setText(s);
    } else {
        QStandardItem* item = new QStandardItem();
        item->setText(s);

        item->setForeground(QBrush(clCL2));
        if( ri % 2 == 0 ) item->setBackground(QBrush(clB1));
        else              item->setBackground(QBrush(clB2));

        item->setFont(QFont("", fontSize));

        wpModel->setItem(ri, ci, item);
        wpTable->setRowHeight(ri, rowHeight);
    }

    return 0;
}

int UI_waypointEdit::updateWaypoints(void)
{
    int     i, n;
    int     heightAltitude;

    int     idx;
    double  alt, heading;
    QString wpType;

    if( cbHeightAltitude->checkState() == Qt::Checked )
        heightAltitude = 1;
    else
        heightAltitude = 0;

    n = wpModel->rowCount();
    for(i=0; i<n; i++) {
        idx     = wpModel->item(i, 0)->text().toInt();
        alt     = wpModel->item(i, 1)->text().toDouble();
        heading = wpModel->item(i, 2)->text().toDouble();
        wpType  = wpModel->item(i, 3)->text();

        if( heightAltitude == 0 ) alt -= m_homeAltitude;

        m_wpMap->value(idx)->SetHeight(alt);
        m_wpMap->value(idx)->SetHomeAlt(m_homeAltitude);
        m_wpMap->value(idx)->SetHeading(heading);
        m_wpMap->value(idx)->SetWaypointType(wpType);
    }

    return 0;
}

void UI_waypointEdit::setReferenceAltitude(double alt)
{
    m_homeAltitude = alt;
}

double UI_waypointEdit::getReferenceAltitude(void)
{
    return m_homeAltitude;
}



void UI_waypointEdit::act_cbAllWaypoints_clicked(bool s)
{
    if( cbAllWaypoints->checkState() == Qt::Checked ) {
        setWaypoints_(-1, m_wpMap, m_bHeightAltitude);
    } else{
        setWaypoints_(m_wpIdx, m_wpMap, m_bHeightAltitude);
    }
}

void UI_waypointEdit::act_cbHeightAltitude_clicked(bool s)
{
    if( cbHeightAltitude->checkState() == Qt::Checked ) {
        m_bHeightAltitude = 1;
    } else {
        m_bHeightAltitude = 0;
    }

    if( cbAllWaypoints->checkState() == Qt::Checked ) {
        setWaypoints_(-1, m_wpMap, m_bHeightAltitude);
    } else {
        setWaypoints_(m_wpIdx, m_wpMap, m_bHeightAltitude);
    }
}

void UI_waypointEdit::act_btnSetAllHeight(void)
{
    // get value from configure file & user input height value
    bool ok = true;
    double &wp_defaultHeight = svar.GetDouble("FastGCS.WP.DefaultHeight", 100.0);

    double h = QInputDialog::getDouble(this,
                                tr("Set all WPs height"),
                                tr("Height (above home altitude [m]):"),
                                wp_defaultHeight, 0, 10000,
                                1,
                                &ok);
    if ( ok ) wp_defaultHeight = h;
    else return;

    // set all WPs height
    QList<int> ids = m_wpMap->keys();

    foreach(int i, ids) {
        setTableItem(i, 1, QString("%1").arg(wp_defaultHeight));
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_diagMissionProgress::UI_diagMissionProgress(QWidget *parent)
    : QDialog(parent)
{
    m_uas = NULL;
    m_bFinished = 0;
    m_iReadWriteType = 0;

    // setup UI controls
    setupUI();

    // start timer
    startTimer(50);
}

UI_diagMissionProgress::~UI_diagMissionProgress()
{
}

void UI_diagMissionProgress::setupUI(void)
{
    setFixedSize(371, 135);
    //resize(371, 135);
    setWindowTitle("Waypoint uploading progress");

    m_progressBar = new QProgressBar(this);
    m_progressBar->setGeometry(QRect(20, 30, 331, 21));
    m_progressBar->setValue(0);

    QLabel *labProgress = new QLabel(this);
    labProgress->setGeometry(QRect(20, 10, 61, 16));
    labProgress->setText("Progress:");

    QLabel *labStatus = new QLabel(this);
    labStatus->setGeometry(QRect(20, 70, 57, 16));
    labStatus->setText("Status:");

    m_labStatus = new QLabel(this);
    m_labStatus->setGeometry(QRect(70, 70, 331, 16));
    m_labStatus->setText("");

    m_buttonBox = new QDialogButtonBox(this);
    m_buttonBox->setObjectName(QString::fromUtf8("buttonBox"));
    m_buttonBox->setGeometry(QRect(20, 95, 331, 32));
    m_buttonBox->setOrientation(Qt::Horizontal);
    m_buttonBox->setStandardButtons(QDialogButtonBox::Ok);

    QObject::connect(m_buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
    QObject::connect(m_buttonBox, SIGNAL(rejected()), this, SLOT(reject()));

    m_buttonBox->setDisabled(true);
}

int UI_diagMissionProgress::setUAS(UAS_Base *u, int t)
{
    m_uas = u;
    m_iReadWriteType = t;

    if( t == 0 )
        setWindowTitle(QString("Waypoint downloading of MAV[%1]").arg(m_uas->ID));
    else
        setWindowTitle(QString("Waypoint uploading of MAV[%1]").arg(m_uas->ID));

    m_bFinished = 0;

    return 0;
}


void UI_diagMissionProgress::timerEvent(QTimerEvent *event)
{
    if( m_uas == NULL || m_bFinished ) return;

    AP_MissionArray                 *wp = &(m_uas->m_waypoints);;
    int                             per, iRW, nRW;
    AP_MissionArray::WP_RW_STATUS   status;

    status = wp->getStatus(iRW, nRW);
    if( nRW <= 1 ) per = 0.0;
    else           per = (int)( 100.0*iRW / (nRW-1) );

    if( m_iReadWriteType == 0 ) {
        // download waypoints from MAV

        if( status == AP_MissionArray::READING_NUM ) {
            per = 0;
            QString s = QString("Request waypoint number");

            m_labStatus->setText(s);
            m_progressBar->setValue(per);
        }

        if( status == AP_MissionArray::READING_ITEM ) {
            QString s = QString("Reading waypoint [%1/%2]").arg(iRW).arg(nRW);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);
        }

        if( status == AP_MissionArray::IDLE ) {
            per = 100;
            QString s = QString("Waypoint downloaded from MAV:%1").arg(m_uas->ID);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);

            m_buttonBox->setDisabled(false);

            m_bFinished = 1;
        }
    } else {
        // uploading waypoints to MAV

        if( status == AP_MissionArray::WRITTING_ITEM ) {
            QString s = QString("Uploading waypoint [%1/%2]").arg(iRW).arg(nRW);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);
        }

        if( status == AP_MissionArray::WRITTING_CONFIRM_ITEM ) {
            QString s = QString("Confirming waypoint [%1/%2]").arg(iRW).arg(nRW);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);
        }

        if( status == AP_MissionArray::IDLE ) {
            per = 100;
            QString s = QString("Confirmed waypoint uploading to MAV:%1").arg(m_uas->ID);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);

            m_buttonBox->setDisabled(false);

            m_bFinished = 1;
        }
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_diagParamProgress::UI_diagParamProgress(QWidget *parent)
    : QDialog(parent)
{
    m_uas = NULL;
    m_bFinished = 0;

    // setup UI controls
    setupUI();

    // start timer
    startTimer(30);
}

UI_diagParamProgress::~UI_diagParamProgress()
{
}

void UI_diagParamProgress::setupUI(void)
{
    setFixedSize(371, 135);
    //resize(371, 135);
    setWindowTitle("Parameter downloading");

    m_progressBar = new QProgressBar(this);
    m_progressBar->setGeometry(QRect(20, 30, 331, 21));
    m_progressBar->setValue(0);

    QLabel *labProgress = new QLabel(this);
    labProgress->setGeometry(QRect(20, 10, 61, 16));
    labProgress->setText("Progress:");

    QLabel *labStatus = new QLabel(this);
    labStatus->setGeometry(QRect(20, 70, 57, 16));
    labStatus->setText("Status:");

    m_labStatus = new QLabel(this);
    m_labStatus->setGeometry(QRect(70, 70, 331, 16));
    m_labStatus->setText("");
}

int UI_diagParamProgress::setUAS(UAS_Base *u, int rw)
{
    m_uas = u;
    m_rw = rw;

    if( m_rw == 0 ) setWindowTitle(QString("Parameter downloading of MAV:%1").arg(u->ID));
    else            setWindowTitle(QString("Parameter uploading of MAV:%1").arg(u->ID));

    return 0;
}


void UI_diagParamProgress::timerEvent(QTimerEvent *event)
{
    if( m_uas == NULL ) return;

    AP_ParamArray *pa = &(m_uas->m_paramArray);
    AP_ParamArray::PARAM_RW_STATUS s = pa->getStatus();
    int per, pn, pi;

    s = pa->getStatus(pn, pi);
    if( pn < 1 ) return;

    if( m_rw == 0 ) {
        if( s == AP_ParamArray::READING || s == AP_ParamArray::READING_ALL || s == AP_ParamArray::READING_BATCH) {
            if( pn > 1 ) per = (int)( 100.0*pi / (pn-1) );
            else         per = 100.0;
            QString s = QString("Reading parameter: [%1/%2]").arg(pi).arg(pn);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);
        }

        if( s == AP_ParamArray::IDLE ) {
            per = 100;
            QString s = QString("Reading parameters finished. MAV:%1, nParam:%2").arg(m_uas->ID).arg(pn);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);

            // close the dialog
            this->accept();
        }
    } else {
        if( s == AP_ParamArray::WRITING || s == AP_ParamArray::WRITING_ALL ) {
            if( pn > 1 ) per = (int)( 100.0*pi / (pn-1) );
            else         per = 100.0;
            QString s = QString("Writting parameter: [%1/%2]").arg(pi).arg(pn);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);
        }

        if( s == AP_ParamArray::IDLE ) {
            per = 100;
            QString s = QString("Writting parameters finished. MAV:%1, nParam:%2").arg(m_uas->ID).arg(pn);

            m_labStatus->setText(s);
            m_progressBar->setValue(per);

            // close the dialog
            this->accept();
        }
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_diagComOpen::UI_diagComOpen(QWidget *parent)
{
    setupUI();
}

UI_diagComOpen::~UI_diagComOpen()
{

}


int UI_diagComOpen::getDevInfo(ComDev_Type &t, QString &v1, QString &v2)
{
    if( rbUART->isChecked() ) {
        t = COMDEV_UART;
        v1 = cbUART_port->currentText();
        v2 = cbUART_baud->currentText();

        return 0;
    }

    if( rbFile->isChecked() ) {
        t = COMDEV_FILE;
        v1 = m_fileName;

        return 0;
    }

    if( rbVUAV->isChecked() ) {
        t = COMDEV_VIRTUAL_UAV;
        v1 = cbVUAV_type->currentText();
        v2 = cbVUAV_ID->currentText();

        return 0;
    }

    if( rbUDP->isChecked() ) {
        t = COMDEV_UDP;
        v1 = QString("%1").arg(sbUDP_port->value());

        return 0;
    }

    return -1;
}


void UI_diagComOpen::setupUI(void)
{
    setFixedSize(391, 280);
    setWindowTitle("Open device");

    labDevice = new QLabel(this);
    labDevice->setObjectName("labDevice");
    labDevice->setGeometry(QRect(14, 10, 57, 15));
    labDevice->setText("Devices:");

    // UART
    rbUART = new QRadioButton(this);
    rbUART->setObjectName("rbUART");
    rbUART->setGeometry(QRect(34, 30, 71, 21));
    rbUART->setText("UART");
    rbUART->setChecked(true);
    cbUART_port = new QComboBox(this);
    cbUART_port->setObjectName("cbUART_port");
    cbUART_port->setGeometry(QRect(114, 30, 115, 23));
    QStringList devList = getComDevices();
    cbUART_port->addItems(devList);

    cbUART_baud = new QComboBox(this);
    cbUART_baud->setObjectName("cbUART_baud");
    cbUART_baud->setGeometry(QRect(242, 30, 95, 23));
    QStringList baudList;
    baudList << "115200" << "57600" <<"38400";
    cbUART_baud->addItems(baudList);

    // FILE
    rbFile = new QRadioButton(this);
    rbFile->setObjectName("rbFile");
    rbFile->setGeometry(QRect(34, 60, 71, 21));
    rbFile->setText("File");
    btnOpen = new QPushButton(this);
    btnOpen->setObjectName("btnOpen");
    btnOpen->setGeometry(QRect(114, 60, 80, 23));
    btnOpen->setText("Open");

    // Virtual UAV
    rbVUAV = new QRadioButton(this);
    rbVUAV->setObjectName("rbVUAV");
    rbVUAV->setGeometry(QRect(34, 90, 71, 21));
    rbVUAV->setText("VUAV");
    cbVUAV_type = new QComboBox(this);
    cbVUAV_type->setObjectName("cbVUAV_type");
    cbVUAV_type->setGeometry(QRect(114, 90, 115, 23));
    QStringList uavTypes;
    uavTypes << "QuadCopter" << "FixWings";
    cbVUAV_type->addItems(uavTypes);

    cbVUAV_ID = new QComboBox(this);
    cbVUAV_ID->setObjectName("cbVUAV_ID");
    cbVUAV_ID->setGeometry(QRect(242, 90, 95, 23));
    QStringList idLists;
    idLists << "1" << "2" <<"3";
    cbVUAV_ID->addItems(idLists);

    // UDP
    rbUDP = new QRadioButton(this);
    rbUDP->setObjectName("rbUDP");
    rbUDP->setGeometry(QRect(34, 120, 61, 21));
    rbUDP->setText("UDP");
    sbUDP_port = new QSpinBox(this);
    sbUDP_port->setObjectName("sbUDP_port");
    sbUDP_port->setGeometry(QRect(114, 118, 205, 24));
    sbUDP_port->setMinimum(1000);
    sbUDP_port->setMaximum(60000);
    sbUDP_port->setValue(5530);

    // Device info
    labDevInfo = new QLabel(this);
    labDevInfo->setObjectName("labDevInfo");
    labDevInfo->setGeometry(QRect(14, 168, 81, 16));
    labDevInfo->setText("Device Info:");
    edtDevInfo = new QLineEdit(this);
    edtDevInfo->setObjectName("edtDevInfo");
    edtDevInfo->setGeometry(QRect(36, 188, 341, 23));
    edtDevInfo->setReadOnly(true);
    QString s = QString("UART: %1, %2").arg(cbUART_port->currentText()).arg(cbUART_baud->currentText());
    edtDevInfo->setText(s);

    // buttons
    buttonBox = new QDialogButtonBox(this);
    buttonBox->setObjectName("buttonBox");
    buttonBox->setGeometry(QRect(36, 230, 341, 32));
    buttonBox->setOrientation(Qt::Horizontal);
    buttonBox->setStandardButtons(QDialogButtonBox::Cancel | QDialogButtonBox::Ok);

    // signals
    QObject::connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
    QObject::connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));

    connect(rbUART, SIGNAL(clicked(bool)),
            this, SLOT(rbUART_clicked(bool)));
    connect(cbUART_port, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(cbUART_port_currentIndexChanged(QString)));
    connect(cbUART_baud, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(cbUART_baud_currentIndexChanged(QString)));

    connect(rbFile, SIGNAL(clicked(bool)),
            this, SLOT(rbFILE_clicked(bool)));
    connect(btnOpen, SIGNAL(clicked(bool)),
            this, SLOT(btnOpen_clicked(bool)));

    connect(rbVUAV, SIGNAL(clicked(bool)),
            this, SLOT(rbVUAV_clicked(bool)));
    connect(cbVUAV_type, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(cbVUAV_type_currentIndexChanged(QString)));
    connect(cbVUAV_ID, SIGNAL(currentIndexChanged(QString)),
            this, SLOT(cbVUAV_ID_currentIndexChanged(QString)));

    connect(rbUDP, SIGNAL(clicked(bool)),
            this, SLOT(rbUDP_clicked(bool)));
    connect(sbUDP_port, SIGNAL(valueChanged(int)),
            this, SLOT(sbUPD_port_valueChanged(int)));
}


void UI_diagComOpen::rbUART_clicked(bool checked)
{
    if( checked ) {
        QString s = QString("UART: %1, %2").arg(cbUART_port->currentText()).arg(cbUART_baud->currentText());
        edtDevInfo->setText(s);
    }
}

void UI_diagComOpen::cbUART_port_currentIndexChanged(const QString &text)
{
    rbUART->setChecked(true);

    QString s = QString("UART: %1, %2").arg(cbUART_port->currentText()).arg(cbUART_baud->currentText());
    edtDevInfo->setText(s);
}

void UI_diagComOpen::cbUART_baud_currentIndexChanged(const QString &text)
{
    rbUART->setChecked(true);

    QString s = QString("UART: %1, %2").arg(cbUART_port->currentText()).arg(cbUART_baud->currentText());
    edtDevInfo->setText(s);
}


void UI_diagComOpen::rbFILE_clicked(bool checked)
{
    if( checked ) {
        QString s = QString("FILE: %1").arg(m_fileName);
        edtDevInfo->setText(s);
    }
}

void UI_diagComOpen::btnOpen_clicked(bool checked)
{
    string dp = svar.GetString("FastGCS.DataPath", "./Data2/FastGCS");
    string flight_fn = path_join(dp, "flight_data");


    QString path = flight_fn.c_str();
    QString fname = QFileDialog::getOpenFileName(this, tr("FastGCS project file"),
                                         path,
                                         tr("Project file (*.cfg *.mvd)"));
    if( fname.size() < 1 ) return;

    m_fileName = fname;
    rbFile->setChecked(true);
    QString s = QString("FILE: %1").arg(m_fileName);
    edtDevInfo->setText(s);
}



void UI_diagComOpen::rbVUAV_clicked(bool checked)
{
    if( checked ) {
        QString s = QString("VUAV: %1, ID=%2").arg(cbVUAV_type->currentText()).arg(cbVUAV_ID->currentText());
        edtDevInfo->setText(s);
    }
}

void UI_diagComOpen::cbVUAV_type_currentIndexChanged(const QString &text)
{
    rbVUAV->setChecked(true);

    QString s = QString("VUAV: %1, ID=%2").arg(cbVUAV_type->currentText()).arg(cbVUAV_ID->currentText());
    edtDevInfo->setText(s);
}

void UI_diagComOpen::cbVUAV_ID_currentIndexChanged(const QString &text)
{
    rbVUAV->setChecked(true);

    QString s = QString("VUAV: %1, ID=%2").arg(cbVUAV_type->currentText()).arg(cbVUAV_ID->currentText());
    edtDevInfo->setText(s);
}



void UI_diagComOpen::rbUDP_clicked(bool checked)
{
    if( checked ) {
        QString s = QString("UDP: port=%1").arg(sbUDP_port->value());
        edtDevInfo->setText(s);
    }
}

void UI_diagComOpen::sbUPD_port_valueChanged(int i)
{
    rbUDP->setChecked(true);
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_rcValueBar::UI_rcValueBar(QWidget *parent) :
    QProgressBar(parent)
{
    m_vMin = 1500;
    m_vMax = 1500;
}

UI_rcValueBar::~UI_rcValueBar()
{

}

void UI_rcValueBar::paintEvent(QPaintEvent *event)
{
    // default drawing
    QProgressBar::paintEvent(event);

    // draw min, max marker
    QPainter painter(this);

    QPen penRed(Qt::red);
    penRed.setWidth(2);
    painter.setPen(penRed);

    int vmin, vmax;
    int w, h;

    vmin = minimum();
    vmax = maximum();
    w = this->width();
    h = this->height();

    int xmin = (m_vMin - vmin)*w/(vmax-vmin);
    painter.drawLine(xmin, 0, xmin, h);
    int xmax = (m_vMax - vmin)*w/(vmax-vmin);
    painter.drawLine(xmax, 0, xmax, h);
}

void UI_rcValueBar::setValue(int value)
{
    if( value < m_vMin ) m_vMin = value;
    if( value > m_vMax ) m_vMax = value;

    QProgressBar::setValue(value);
}

void UI_rcValueBar::setMinMaxVal(int vMin, int vMax)
{
    m_vMin = vMin;
    m_vMax = vMax;
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_rcViewWidget::UI_rcViewWidget(QWidget *parent) :
    QWidget(parent)
{
    m_rcChannel = 1;

    m_rcMin = 1500;
    m_rcMax = 1500;
    m_rcVal = 1500;

    m_rcDZ  = 0;
    m_rcTrim = 1500;
    m_trimMode = 1;

    setupUi();
}

UI_rcViewWidget::~UI_rcViewWidget()
{

}

void UI_rcViewWidget::setupUi(void)
{
    this->resize(m_widgetW, m_widgetH);
    this->setMinimumSize(m_widgetW, m_widgetH);

    rcGroup = new QGroupBox(this);
    rcGroup->setObjectName(QString::fromUtf8("rcGroup"));
    rcGroup->setGeometry(QRect(1, 1, 501, 71));
    rcGroup->setFlat(true);
    rcGroup->setCheckable(false);

    pbRCValue = new UI_rcValueBar(rcGroup);
    pbRCValue->setObjectName(QString::fromUtf8("pbRCValue"));
    pbRCValue->setGeometry(QRect(10, 20, 471, 20));
    pbRCValue->setMinimum(svar.GetInt("FastGCS.UI.rcView.rcMin", 800));
    pbRCValue->setMaximum(svar.GetInt("FastGCS.UI.rcView.rcMax", 2200));
    pbRCValue->setFormat("%v");
    pbRCValue->setValue(m_rcVal);

    labDZ = new QLabel(rcGroup);
    labDZ->setObjectName(QString::fromUtf8("labDZ"));
    labDZ->setGeometry(QRect(10, 51, 31, 17));
    labDZ->setText("DZ:");
    labTrim = new QLabel(rcGroup);
    labTrim->setObjectName(QString::fromUtf8("labTrim"));
    labTrim->setGeometry(QRect(100, 51, 41, 17));
    labTrim->setText("TRIM:");

    leDZ = new QLineEdit(rcGroup);
    leDZ->setObjectName(QString::fromUtf8("leDZ"));
    leDZ->setGeometry(QRect(40, 50, 51, 21));
    leTrim = new QLineEdit(rcGroup);
    leTrim->setObjectName(QString::fromUtf8("leTrim"));
    leTrim->setGeometry(QRect(140, 50, 91, 21));
    leTrim->setReadOnly(true);
    leInfo = new QLineEdit(rcGroup);
    leInfo->setObjectName(QString::fromUtf8("leInfo"));
    leInfo->setGeometry(QRect(240, 50, 241, 21));
    leInfo->setReadOnly(true);

    leDZ->setText(QString("%1").arg(m_rcDZ));
    leTrim->setText(QString("%1").arg(m_rcTrim));
}

void UI_rcViewWidget::setRCChannel(int c)
{
    m_rcChannel = c;
    rcGroup->setTitle(QString("RC %1").arg(c));
}


void UI_rcViewWidget::setRCVal(int v)
{
    if( v > m_rcMax ) m_rcMax = v;
    if( v < m_rcMin ) m_rcMin = v;

    if( m_trimMode == 0 )       m_rcTrim = m_rcMin;
    else if( m_trimMode == 1 )  m_rcTrim = (m_rcMin + m_rcMax)/2;
    else                        m_rcTrim = m_rcMax;

    pbRCValue->setValue(v);
    leTrim->setText(QString("%1").arg(m_rcTrim));
    leInfo->setText(QString("MIN:%1, MAX:%2").arg(m_rcMin).arg(m_rcMax));
}

void UI_rcViewWidget::setSettings(int rcMin, int rcMax, int rcDZ, int rcTrim)
{
    m_rcMin = rcMin;
    m_rcMax = rcMax;
    m_rcDZ  = rcDZ;
    m_rcTrim = rcTrim;

    leDZ->setText(QString("%1").arg(m_rcDZ));
    leTrim->setText(QString("%1").arg(m_rcTrim));
    leInfo->setText(QString("MIN:%1, MAX:%2").arg(m_rcMin).arg(m_rcMax));
}

void UI_rcViewWidget::getSettings(int &rcMin, int &rcMax, int &rcDZ, int &rcTrim)
{
    m_rcDZ = leDZ->text().toInt();

    rcMin   = m_rcMin;
    rcMax   = m_rcMax;
    rcDZ    = m_rcDZ;
    rcTrim  = m_rcTrim;
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_rcConfigControl::UI_rcConfigControl(QWidget *parent) :
    QWidget(parent)
{
    m_oldRCRawFreq = 2;
    m_isCalibration = 0;

    for(int i=0; i<8; i++) m_rcRaw[i] = 1500;

    setupUI();

    startTimer(100);
}

UI_rcConfigControl::~UI_rcConfigControl()
{

}

void UI_rcConfigControl::setupUI(void)
{
    this->setObjectName(QString::fromUtf8("UI_rcConfigControl"));
    this->setFixedSize(660, 670);
    this->setMinimumSize(QSize(660, 670));
    this->setWindowTitle("RC Configure");

    // create rc view widget
    int     x, y, w, h, space;

    x = 10; y = 10;
    space = 10;
    w = UI_rcViewWidget::m_widgetW;
    h = UI_rcViewWidget::m_widgetH;

    for(int i=0; i<8; i++) {
        UI_rcViewWidget *rcv = new UI_rcViewWidget(this);
        rcv->setGeometry(x, y, w, h);
        rcv->setRCChannel(i+1);

        rcViewers.push_back(rcv);

        y += space + h;
    }

    rcViewers[0]->setTrimMode(1);
    rcViewers[1]->setTrimMode(1);
    rcViewers[2]->setTrimMode(0);
    rcViewers[3]->setTrimMode(1);
    rcViewers[4]->setTrimMode(0);
    rcViewers[5]->setTrimMode(2);
    rcViewers[6]->setTrimMode(0);
    rcViewers[7]->setTrimMode(0);


    // create buttons
    pbBegin = new QPushButton(this);
    pbBegin->setObjectName(QString::fromUtf8("pbBegin"));
    pbBegin->setText("Begin");
    pbBegin->setGeometry(QRect(520, 10, 131, 23));
    pbEnd = new QPushButton(this);
    pbEnd->setObjectName(QString::fromUtf8("pbEnd"));
    pbEnd->setText("End");
    pbEnd->setGeometry(QRect(520, 43, 131, 23));
    pbWrite = new QPushButton(this);
    pbWrite->setObjectName(QString::fromUtf8("pbWrite"));
    pbWrite->setText("Write");
    pbWrite->setToolTip("Write configures to device");
    pbWrite->setGeometry(QRect(520, 76, 131, 23));

    pbClose = new QPushButton(this);
    pbClose->setObjectName(QString::fromUtf8("pbClose"));
    pbClose->setText("Close");
    pbClose->setGeometry(QRect(520, 110, 131, 23));

    // connect signals
    connect(pbBegin, SIGNAL(clicked(bool)),
            this, SLOT(btnBegin_clicked(bool)));
    connect(pbEnd, SIGNAL(clicked(bool)),
            this, SLOT(btnEnd_clicked(bool)));
    connect(pbWrite, SIGNAL(clicked(bool)),
            this, SLOT(btnWrite_clicked(bool)));

    connect(pbClose, SIGNAL(clicked(bool)),
            this, SLOT(btnClose_clicked(bool)));
}


void UI_rcConfigControl::timerEvent(QTimerEvent *event)
{
    for(int i=0; i<8; i++) {
        if( m_rcRaw[i] > 800 ) rcViewers[i]->setRCVal(m_rcRaw[i]);
    }
}

void UI_rcConfigControl::closeEvent(QCloseEvent *event)
{
    _close();
}


void UI_rcConfigControl::btnBegin_clicked(bool checked)
{
    if( m_uas == NULL ) return;

    if( m_isCalibration ) return;

    // get old offsets
    AP_ParamItem *pi;

    if( m_uas->m_paramArray.isLoaded() ) {
        for(int i=0; i<8; i++) {
            char buf[256];

            sprintf(buf, "RC%d", i+1);

            pi = m_uas->m_paramArray.get(fmt::sprintf("%s_DZ", buf));
            if( pi != NULL ) rcViewers[i]->setDZ(pi->toInt32());

            pi = m_uas->m_paramArray.get(fmt::sprintf("%s_TRIM", buf));
            if( pi != NULL ) rcViewers[i]->setTrim(pi->toInt32());
        }
    } else {
        QMessageBox::warning(this,
                             tr("Parameter not load"),
                             tr("Please load parameters first!"));
        return;
    }


    // set raw RC data frequency
    m_oldRCRawFreq = m_uas->getStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS);
    m_uas->setStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS, 10);

    // regist mavlink message handle
    Mavlink_Message_Handle h = tr1::bind(&UI_rcConfigControl::parseMavlinkMsg,
                                         this, std::tr1::placeholders::_1);
    m_uas->registMavlinkMessageHandle("UI_rcConfigControl", h);

    m_isCalibration = 1;
}

void UI_rcConfigControl::btnEnd_clicked(bool checked)
{
    if( m_uas == NULL ) return;

    if( !m_isCalibration ) return;

    m_uas->setStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS, m_oldRCRawFreq);
    m_uas->unregistMavlinkMessageHandle("UI_rcConfigControl");

    m_isCalibration = 0;

    for(int i=0; i<8; i++) {
        int dz, trim, vmin, vmax;

        rcViewers[i]->getSettings(vmin, vmax, dz, trim);
        dbg_pi("RC_CHANNEL[%2d] vmin, vmax = [%4d ~ %4d], DZ: %4d, TRIM: %4d\n",
               i+1, vmin, vmax, dz, trim);
    }
}

void UI_rcConfigControl::btnWrite_clicked(bool checked)
{
    if( m_uas == NULL ) return;

    if( m_isCalibration ) return;

    if( !m_uas->m_paramArray.isLoaded() ) {
        QMessageBox::warning(this,
                             tr("RC Calibration Failed"),
                             tr("Please load parameters first!"));
        return;
    }

    // update values
    AP_ParamItem    *pi;
    StringArray     sa;
    string          varName;

    if( m_uas->m_paramArray.isLoaded() ) {
        for(int i=0; i<8; i++) {
            char buf[256];
            int vmin, vmax, dz, trim;

            rcViewers[i]->getSettings(vmin, vmax, dz, trim);

            sprintf(buf, "RC%d", i+1);

            varName = fmt::sprintf("%s_MIN", buf);
            pi = m_uas->m_paramArray.get(varName); if( pi != NULL ) pi->fromInt32(vmin);
            sa.push_back(varName);

            varName = fmt::sprintf("%s_MAX", buf);
            pi = m_uas->m_paramArray.get(varName); if( pi != NULL ) pi->fromInt32(vmax);
            sa.push_back(varName);

            varName = fmt::sprintf("%s_DZ", buf);
            pi = m_uas->m_paramArray.get(varName); if( pi != NULL ) pi->fromInt32(dz);
            sa.push_back(varName);

            varName = fmt::sprintf("%s_TRIM", buf);
            pi = m_uas->m_paramArray.get(varName); if( pi != NULL ) pi->fromInt32(trim);
            sa.push_back(varName);
        }
    }

    // upload to device
    m_uas->m_paramArray.updateParameters(sa);
}

void UI_rcConfigControl::btnClose_clicked(bool checked)
{
    _close();

    this->close();
}

int UI_rcConfigControl::setUAS(UAS_Base *uas)
{
    m_uas = uas;
}

int UI_rcConfigControl::parseMavlinkMsg(mavlink_message_t *msg)
{
    if( msg->msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW ) {
        mavlink_rc_channels_raw_t msg_rc_raw;
        mavlink_msg_rc_channels_raw_decode(msg, &msg_rc_raw);

        m_rcRaw[0] = msg_rc_raw.chan1_raw;
        m_rcRaw[1] = msg_rc_raw.chan2_raw;
        m_rcRaw[2] = msg_rc_raw.chan3_raw;
        m_rcRaw[3] = msg_rc_raw.chan4_raw;
        m_rcRaw[4] = msg_rc_raw.chan5_raw;
        m_rcRaw[5] = msg_rc_raw.chan6_raw;
        m_rcRaw[6] = msg_rc_raw.chan7_raw;
        m_rcRaw[7] = msg_rc_raw.chan8_raw;

        dbg_pi("RC %4d, %4d %4d %4d, %4d, %4d %4d %4d\n",
               m_rcRaw[0], m_rcRaw[1], m_rcRaw[2], m_rcRaw[3],
               m_rcRaw[4], m_rcRaw[5], m_rcRaw[6], m_rcRaw[7]);
    }
}

void UI_rcConfigControl::_close()
{
    if( m_uas && m_isCalibration ) {
        m_uas->setStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS, m_oldRCRawFreq);
        m_uas->unregistMavlinkMessageHandle("UI_rcConfigControl");

        m_isCalibration = 0;
    }
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_flightModeWidget::UI_flightModeWidget(QWidget *parent) :
    QWidget(parent)
{
    m_modeIndex = 1;
    m_isActive = 0;

    //Note: not all modes are used in utils_mavlink enum, neither here
    //refer to setupUI() below to see really displayed modes
    m_flightModeStrings.push_back("Manual");
    m_flightModeStrings.push_back("AltCtl");
    m_flightModeStrings.push_back("PosCtl");
    m_flightModeStrings.push_back("Auto_mission");
    m_flightModeStrings.push_back("Auto_loiter");
    m_flightModeStrings.push_back("Auto_RTL");
    m_flightModeStrings.push_back("Acro");
    m_flightModeStrings.push_back("OffBoard");
    m_flightModeStrings.push_back("Stabilized");
    m_flightModeStrings.push_back("Auto_takeoff");
    m_flightModeStrings.push_back("Auto_land");
    m_flightModeStrings.push_back("Auto_follow_target");
    m_flightModeStrings.push_back("Auto_orbit");


    m_flightModeMap["Manual"]               = 0;
    m_flightModeMap["AltCtl"]               = 1;
    m_flightModeMap["PosCtl"]               = 2;
    m_flightModeMap["Auto_mission"]         = 3;
    m_flightModeMap["Auto_loiter"]          = 4;
    m_flightModeMap["Auto_RTL"]             = 5;
    m_flightModeMap["Acro"]                 = 7;
    m_flightModeMap["OffBoard"]             = 10;
    m_flightModeMap["Stabilized"]           = 11;
    m_flightModeMap["Auto_takeoff"]         = 12;
    m_flightModeMap["Auto_land"]            = 13;
    m_flightModeMap["Auto_follow_target"]   = 14;
    m_flightModeMap["Auto_orbit"]           = 16;

    setupUi();
}

UI_flightModeWidget::~UI_flightModeWidget()
{

}

void UI_flightModeWidget::setupUi(void)
{
    this->resize(m_widgetW, m_widgetH);
    this->setMinimumSize(m_widgetW, m_widgetH);

    labModeIndex = new QLabel(this);
    labModeIndex->setObjectName(QString::fromUtf8("ModeIndex"));
    labModeIndex->setGeometry(QRect(10, 12, 91, 17));
    labModeIndex->setText(QString("Flight Mode %1:").arg(m_modeIndex));

    cbFlightType = new QComboBox(this);
    cbFlightType->setObjectName(QString::fromUtf8("FlightMode"));
    cbFlightType->setGeometry(QRect(100, 10, 221, 21));
    cbFlightType->addItems(m_flightModeStrings);
    cbFlightType->setEditable(false);

    setActive(0);
}

void UI_flightModeWidget::setModeIndex(int c)
{
    m_modeIndex = c;
    labModeIndex->setText(QString("Flight Mode %1:").arg(m_modeIndex));
}

void UI_flightModeWidget::setActive(int active)
{
    m_isActive = active;

    QPalette p = labModeIndex->palette();

    if( m_isActive ) p.setColor(labModeIndex->foregroundRole(), Qt::red);
    else             p.setColor(labModeIndex->foregroundRole(), Qt::black);

    labModeIndex->setPalette(p);
}

void UI_flightModeWidget::setFlightMode(int fm)
{
    std::map<std::string, int>::iterator it;
    std::string szFM = "";

    for(it=m_flightModeMap.begin(); it!=m_flightModeMap.end(); it++) {
        if( fm == it->second ) {
            szFM = it->first;
            break;
        }
    }

    if( szFM.size() > 0 ) {
        int index = cbFlightType->findText(QString::fromStdString(szFM), Qt::MatchExactly);
        if( index >= 0 ) cbFlightType->setCurrentIndex(index);
    }
}

int  UI_flightModeWidget::getFlightMode(void)
{
    return m_flightModeMap[cbFlightType->currentText().toStdString()];
}




UI_flightModeConfig::UI_flightModeConfig(QWidget *parent) :
    QWidget(parent)
{
    m_uas = NULL;
    m_oldRCRawFreq = 2;
    m_flightModeChannel = 4;

    int i;
    for(i=0; i<m_flightModes; i++) m_fmControls[i] = NULL;
    for(i=0; i<8; i++) m_rcRaw[i] = 0;

    setupUI();

    startTimer(50);
}

UI_flightModeConfig::~UI_flightModeConfig()
{
    m_uas = NULL;
}

void UI_flightModeConfig::setupUI(void)
{
    int widgetW = 350;
    int widgetH = 350;

    this->resize(widgetW, widgetH);
    this->setMinimumSize(widgetW, widgetH);
    this->setMaximumSize(widgetW, widgetH);
    this->setWindowTitle("Flight mode config");

    // flight mode widgets
    int x, y;
    x = 10;
    y = 10;

    for(int i=0; i<m_flightModes; i++) {
        m_fmControls[i] = new UI_flightModeWidget(this);
        m_fmControls[i]->setGeometry(QRect(x, y, UI_flightModeWidget::m_widgetW, UI_flightModeWidget::m_widgetH));

        m_fmControls[i]->setModeIndex(i+1);
        m_fmControls[i]->setActive(0);

        y += UI_flightModeWidget::m_widgetH + 10;
    }

    // buttons
    btnWrite = new QPushButton(this);
    btnWrite->setGeometry(QRect());
    btnWrite->setObjectName("btnWrite");
    btnWrite->setGeometry(QRect(160, 310, 80, 23));
    btnWrite->setText("Write");

    btnClose = new QPushButton(this);
    btnClose->setGeometry(QRect());
    btnClose->setObjectName("btnClose");
    btnClose->setGeometry(QRect(250, 310, 80, 23));
    btnClose->setText("Closes");

    connect(btnWrite, SIGNAL(clicked(bool)), this, SLOT(btnWrite_clicked(bool)));
    connect(btnClose, SIGNAL(clicked(bool)), this, SLOT(btnClose_clicked(bool)));
}

int UI_flightModeConfig::setUAS(UAS_Base *uas)
{
    release();

    m_uas = uas;
    if( m_uas != NULL ) {
        // set raw RC data frequency
        m_oldRCRawFreq = m_uas->getStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS);
        m_uas->setStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS, 10);

        // regist mavlink message handle
        Mavlink_Message_Handle h = tr1::bind(&UI_flightModeConfig::parseMavlinkMsg,
                                             this, std::tr1::placeholders::_1);
        m_uas->registMavlinkMessageHandle("UI_flightModeConfig", h);

        // get exist flight mode settings
        for(int i=0; i<m_flightModes; i++) {
            AP_ParamItem *pi = m_uas->m_paramArray.get(fmt::sprintf("FLTMODE%d", i+1));
            if( pi != NULL ) {
                m_fmControls[i]->setFlightMode(pi->toInt32());
            }
        }
    }
}

int UI_flightModeConfig::parseMavlinkMsg(mavlink_message_t *msg)
{
    if( msg->msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW ) {
        mavlink_rc_channels_raw_t msg_rc_raw;
        mavlink_msg_rc_channels_raw_decode(msg, &msg_rc_raw);

        m_rcRaw[0] = msg_rc_raw.chan1_raw;
        m_rcRaw[1] = msg_rc_raw.chan2_raw;
        m_rcRaw[2] = msg_rc_raw.chan3_raw;
        m_rcRaw[3] = msg_rc_raw.chan4_raw;
        m_rcRaw[4] = msg_rc_raw.chan5_raw;
        m_rcRaw[5] = msg_rc_raw.chan6_raw;
        m_rcRaw[6] = msg_rc_raw.chan7_raw;
        m_rcRaw[7] = msg_rc_raw.chan8_raw;

        /*
        dbg_pi("RC %4d, %4d %4d %4d, %4d, %4d %4d %4d\n",
               m_rcRaw[0], m_rcRaw[1], m_rcRaw[2], m_rcRaw[3],
               m_rcRaw[4], m_rcRaw[5], m_rcRaw[6], m_rcRaw[7]);
        */
    }
}

int UI_flightModeConfig::release(void)
{
    if( m_uas != NULL ) {
        m_uas->setStreamFrequency(MAV_DATA_STREAM_RC_CHANNELS, m_oldRCRawFreq);
        m_uas->unregistMavlinkMessageHandle("UI_flightModeConfig");

        m_uas = NULL;
    }

    return 0;
}

void UI_flightModeConfig::btnWrite_clicked(bool checked)
{
    if( m_uas == NULL ) return;

    // get exist flight mode settings
    for(int i=0; i<m_flightModes; i++) {
        int fm = m_fmControls[i]->getFlightMode();

        AP_ParamItem *pi = m_uas->m_paramArray.get(fmt::sprintf("FLTMODE%d", i+1));
        if( pi != NULL ) pi->fromInt32(fm);
    }

    // upload to device
    StringArray sa;
    sa.push_back("FLTMODE1");   sa.push_back("FLTMODE2");   sa.push_back("FLTMODE3");
    sa.push_back("FLTMODE4");   sa.push_back("FLTMODE5");   sa.push_back("FLTMODE6");
    m_uas->m_paramArray.updateParameters(sa);
}

void UI_flightModeConfig::btnClose_clicked(bool checked)
{
    release();
    this->close();
}

void UI_flightModeConfig::timerEvent(QTimerEvent *event)
{
    for(int i=0; i<m_flightModes; i++) m_fmControls[i]->setActive(0);

    int fmPWM = m_rcRaw[m_flightModeChannel];
    int fmIndex = 0;

    if( fmPWM <= 1230)                          fmIndex = 0;
    else if( fmPWM > 1230 && fmPWM <= 1360 )    fmIndex = 1;
    else if( fmPWM > 1360 && fmPWM <= 1490 )    fmIndex = 2;
    else if( fmPWM > 1490 && fmPWM <= 1620 )    fmIndex = 3;
    else if( fmPWM > 1620 && fmPWM <= 1749 )    fmIndex = 4;
    else if( fmPWM > 1749 )                     fmIndex = 5;

    m_fmControls[fmIndex]->setActive(1);
}

void UI_flightModeConfig::closeEvent(QCloseEvent *event)
{
    release();
    this->close();
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_diagAccelConfig::UI_diagAccelConfig(QWidget *parent) :
    QWidget(parent)
{
    m_uas = NULL;

    setupUI();
}

UI_diagAccelConfig::~UI_diagAccelConfig()
{
    m_uas = NULL;
}

void UI_diagAccelConfig::setupUI(void)
{
    setFixedSize(400, 140);
    setWindowTitle("Accelerator Config");

    // message label & edit
    labMsg = new QLabel(this);
    labMsg->setObjectName("labMsg");
    labMsg->setGeometry(QRect(14, 13, 57, 15));
    labMsg->setText("Message:");

    leMsg = new QLineEdit(this);
    leMsg->setObjectName("leMsg");
    leMsg->setGeometry(QRect(76, 10, 311, 23));
    leMsg->setReadOnly(true);
    QString s = QString("Ready to calibrate");
    leMsg->setText(s);

    // buttons
    btnBegin = new QPushButton(this);
    btnBegin->setObjectName("btnBegin");
    btnBegin->setGeometry(QRect(14, 50, 80, 23));
    btnBegin->setText("Begin");

    btnNext = new QPushButton(this);
    btnNext->setObjectName("btnNext");
    btnNext->setGeometry(QRect(100, 50, 80, 23));
    btnNext->setText("Next");

    btnClose = new QPushButton(this);
    btnClose->setObjectName("btnClose");
    btnClose->setGeometry(QRect(186, 50, 80, 23));
    btnClose->setText("Close");

    // signals
    connect(btnBegin, SIGNAL(clicked(bool)), this, SLOT(btnBegin_clicked(bool)));
    connect(btnNext, SIGNAL(clicked(bool)), this, SLOT(btnNext_clicked(bool)));
    connect(btnClose, SIGNAL(clicked(bool)), this, SLOT(btnClose_clicked(bool)));
}

int UI_diagAccelConfig::setUAS(UAS_Base *uas)
{
    m_uas = uas;

    return 0;
}

int UI_diagAccelConfig::parseMavlinkMsg(mavlink_message_t *msg)
{
    switch( msg->msgid ) {
    case MAVLINK_MSG_ID_STATUSTEXT:
    {
        mavlink_statustext_t st;
        mavlink_msg_statustext_decode(msg, &st);

        msgSeverity = st.severity;
        strcpy(msgText, st.text);

        leMsg->setText(QString::fromStdString(trim(msgText)));
        dbg_pi("AccelConfig: %s", msgText);

        break;
    }
    }

    return 0;
}

int UI_diagAccelConfig::release(void)
{
    if( m_uas != NULL )
        m_uas->unregistMavlinkMessageHandle("UI_diagAccelConfig");
}

void UI_diagAccelConfig::btnBegin_clicked(bool checked)
{
    if( m_uas == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();
        return;
    }

    // regist mavlink message handle
    Mavlink_Message_Handle h = tr1::bind(&UI_diagAccelConfig::parseMavlinkMsg,
                                         this, std::tr1::placeholders::_1);
    m_uas->registMavlinkMessageHandle("UI_diagAccelConfig", h);

    // send gyro calibration command
    {
        leMsg->setText("Init gyro");
        dbg_pi("Init gyro");

        MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
        int confirm = 0;
        float param1 = 1.0;
        float param2 = 0.0;
        float param3 = 0.0;
        float param4 = 0.0;
        float param5 = 0.0;
        float param6 = 0.0;
        float param7 = 0.0;
        int component = 1;

        m_uas->executeCommand(command, confirm,
                              param1, param2, param3, param4, param5, param6, param7,
                              component);

        tm_sleep(50);
    }

    // send accelerator calibration command
    m_accelAckCount = 0;

    {
        leMsg->setText("Begin calibrate accelerator");
        dbg_pi("Begin calibrate accelerator");

        MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
        int confirm = 0;
        float param1 = 0.0;
        float param2 = 0.0;
        float param3 = 0.0;
        float param4 = 0.0;
        float param5 = 1.0;
        float param6 = 0.0;
        float param7 = 0.0;
        int component = 1;

        m_uas->executeCommand(command, confirm,
                              param1, param2, param3, param4, param5, param6, param7,
                              component);
    }
}

void UI_diagAccelConfig::btnNext_clicked(bool checked)
{
    if( m_uas == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();
        return;
    }

    m_uas->executeCommandAck(m_accelAckCount++, true);
}

void UI_diagAccelConfig::btnClose_clicked(bool checked)
{
    release();
    this->close();
}

void UI_diagAccelConfig::closeEvent(QCloseEvent *event)
{
    release();
    this->close();
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_compassViewer::UI_compassViewer(QWidget *parent) :
    QGLViewer(parent)
{
    m_maxLeng = -100;

    m_viewDis   = 4;
    m_viewPitch = 30;
    m_viewYaw   = 0;

    this->resize(300, 300);
    this->setMinimumSize(QSize(300, 300));

    setSceneCenter(qglviewer::Vec(0, 0, 0));
    setSceneRadius(1.5);
    showEntireScene();

    startTimer(30);
}

int UI_compassViewer::addData(Point3d &dat)
{
    ScopedMutex m(m_mutex);

    m_data.push_back(dat);

    if( fabs(dat.x) > m_maxLeng ) m_maxLeng = fabs(dat.x);
    if( fabs(dat.y) > m_maxLeng ) m_maxLeng = fabs(dat.y);
    if( fabs(dat.z) > m_maxLeng ) m_maxLeng = fabs(dat.z);

    return 0;
}

int UI_compassViewer::clearData(void)
{
    ScopedMutex m(m_mutex);

    m_data.clear();

    m_maxLeng = -100;

    return 0;
}

void UI_compassViewer::draw(void)
{
    glDisable(GL_LIGHTING);
    glDisable(GL_POINT_SMOOTH);

    // draw axis
    glColor3f(1, 1, 1);
    drawAxis(1.2);

    {
        ScopedMutex m(m_mutex);
        vector<Point3d>::iterator it;
        int  i, n;

        n = m_data.size();
        if( n < 1 ) return;

        glPointSize(5.0);
        glBegin(GL_POINTS);

        for(i=0, it=m_data.begin(); it!=m_data.end(); i++, it++) {
            if( i == n - 1 ) break;

            glColor3f(0, 1, 1);
            glVertex3d(it->x/m_maxLeng, it->y/m_maxLeng, it->z/m_maxLeng);
        }

        glEnd();

        // draw last point
        Point3d &p = m_data.back();

        glPointSize(10.0);
        glBegin(GL_POINTS);
        glColor3f(1, 0, 0);
        glVertex3d(p.x/m_maxLeng, p.y/m_maxLeng, p.z/m_maxLeng);
        glEnd();
    }
}

QString UI_compassViewer::helpString() const
{
    QString text("<h2>Compass data viewer</h2>");
    return text;
}

void UI_compassViewer::timerEvent(QTimerEvent *event)
{
    double  l;
    qglviewer::Vec vp, vh, vu;

    l  = m_viewDis*cos(m_viewPitch*DEG2RAD);
    vp.setValue(l*cos(m_viewYaw*DEG2RAD),
                l*sin(m_viewYaw*DEG2RAD),
                m_viewDis*sin(m_viewPitch*DEG2RAD));
    vh.setValue(cos((m_viewYaw+90)*DEG2RAD),
                sin((m_viewYaw+90)*DEG2RAD),
                0);

    vu = vp^vh;

    this->camera()->setPosition(vp);
    this->camera()->setViewDirection(-vp);
    this->camera()->setUpVector(vu);

    updateGL();

    m_viewYaw += 1.5;
}

void UI_compassViewer::keyPressEvent(QKeyEvent *e)
{
    // Get event modifiers key
    const Qt::KeyboardModifiers modifiers = e->modifiers();

    // A simple switch on e->key() is not sufficient if we want to take state key into account.
    // With a switch, it would have been impossible to separate 'F' from 'CTRL+F'.
    // That's why we use imbricated if...else and a "handled" boolean.

    if ((e->key()==Qt::Key_W) && (modifiers==Qt::NoButton)) {
        this->camera()->setPosition(qglviewer::Vec(0, 0, 1));
        this->camera()->setViewDirection(qglviewer::Vec(0, 0, -1));
        this->camera()->setUpVector(qglviewer::Vec(0, 1, 0));
        showEntireScene();

        updateGL();
    }
    else if ( e->key() == Qt::Key_Escape ) {

    }
    else if ( (e->key() == Qt::Key_R) && (modifiers==Qt::NoButton) ) {
        showEntireScene();
    }
    else {
        QGLViewer::keyPressEvent(e);
    }
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


void fittingSphere_Error(const alglib::real_1d_array &xi, alglib::real_1d_array &fi, void *obj)
{
    double xofs = xi[0];
    double yofs = xi[1];
    double zofs = xi[2];
    double r    = xi[3];
    int a = 0;

    vector<Point3d> rawImuVector = *reinterpret_cast<vector<Point3d>*>(obj);

    for(int count = 0; count < rawImuVector.size() ; count ++) {
        Point3d d = rawImuVector[count];
        double x = d.x;
        double y = d.y;
        double z = d.z;
        double err = r - sqrt(pow((x + xofs), 2) + pow((y + yofs), 2) + pow((z + zofs), 2));
        fi[a] = err;
        a++;
    }
}

pi::Point3d fittingSphere(std::vector<Point3d> &dat)
{
    using namespace alglib;

    real_1d_array x("[0.0 , 0.0 , 0.0 , 0.0]");
    double epsg = 0.0000000001;
    double epsf = 0;
    double epsx = 0;
    int maxits = 0;
    alglib::minlmstate state;
    alglib::minlmreport rep;

    alglib::minlmcreatev(dat.size(), x, 100.0f,  state);
    alglib::minlmsetcond(state, epsg, epsf, epsx, maxits);
    alglib::minlmoptimize(state, &fittingSphere_Error, NULL, &dat);
    alglib::minlmresults(state, x, rep);

    dbg_pt("rep.terminationType = %d", rep.terminationtype);

    Point3d v(x[0], x[1], x[2]);
    return v;
}

UI_compassConfigControl::UI_compassConfigControl(QWidget *parent) :
    QWidget(parent)
{
    old_streamRawSensorsFreq = 2;
    m_isCalibration = 0;

    m_offNew1.x = 0; m_offNew1.y = 0; m_offNew1.z = 0;
    m_offNew2.x = 0; m_offNew2.y = 0; m_offNew2.z = 0;

    m_devID1 = 0;
    m_devID2 = 0;

    setupUI();

    startTimer(100);
}

UI_compassConfigControl::~UI_compassConfigControl()
{

}

void UI_compassConfigControl::setupUI(void)
{
    this->setObjectName(QString::fromUtf8("CompassConfigControl"));
    this->resize(800, 600);
    this->setMinimumSize(QSize(800, 600));
    this->setWindowTitle("Compass Configure");

    QWidget *configPanel = new QWidget(this);
    configPanel->setMinimumSize(QSize(780, 185));

    gpCompass1 = new QGroupBox(configPanel);
    gpCompass1->setObjectName(QString::fromUtf8("gpCompass1"));
    gpCompass1->setGeometry(QRect(10, 10, 780, 61));
    gpCompass1->setTitle("Compass 1");
    gpCompass1->setFlat(false);
    labOri1 = new QLabel(gpCompass1);
    labOri1->setObjectName(QString::fromUtf8("labOri1"));
    labOri1->setText("Oriention:");
    labOri1->setGeometry(QRect(10, 30, 81, 16));
    cbOri1 = new QComboBox(gpCompass1);
    cbOri1->setObjectName(QString::fromUtf8("cbOri1"));
    cbOri1->setGeometry(QRect(70, 26, 141, 23));
    labOff1 = new QLabel(gpCompass1);
    labOff1->setObjectName(QString::fromUtf8("labOff1"));
    labOff1->setText("Offset:");
    labOff1->setGeometry(QRect(230, 30, 81, 16));
    leOri1 = new QLineEdit(gpCompass1);
    leOri1->setObjectName(QString::fromUtf8("leOri1"));
    leOri1->setGeometry(QRect(275, 26, 450, 23));
    leOri1->setReadOnly(true);

    gpCompass2 = new QGroupBox(configPanel);
    gpCompass2->setObjectName(QString::fromUtf8("gpCompass2"));
    gpCompass2->setTitle("Compass 2");
    gpCompass2->setGeometry(QRect(10, 80, 780, 61));
    gpCompass2->setFlat(false);
    labOri2 = new QLabel(gpCompass2);
    labOri2->setObjectName(QString::fromUtf8("labOri2"));
    labOri2->setText("Oriention:");
    labOri2->setGeometry(QRect(10, 30, 81, 16));
    cbOri2 = new QComboBox(gpCompass2);
    cbOri2->setObjectName(QString::fromUtf8("cbOri2"));
    cbOri2->setGeometry(QRect(70, 26, 141, 23));
    labOff2 = new QLabel(gpCompass2);
    labOff2->setObjectName(QString::fromUtf8("labOff2"));
    labOff2->setText("Offset:");
    labOff2->setGeometry(QRect(230, 30, 81, 16));
    leOri2 = new QLineEdit(gpCompass2);
    leOri2->setObjectName(QString::fromUtf8("lbOri2"));
    leOri2->setGeometry(QRect(275, 26, 450, 23));
    leOri2->setReadOnly(true);

    pbBegin = new QPushButton(configPanel);
    pbBegin->setObjectName(QString::fromUtf8("pbBegin"));
    pbBegin->setText("Begin");
    pbBegin->setGeometry(QRect(10, 160, 131, 23));
    pbEnd = new QPushButton(configPanel);
    pbEnd->setObjectName(QString::fromUtf8("pbEnd"));
    pbEnd->setText("End");
    pbEnd->setGeometry(QRect(150, 160, 131, 23));
    pbCancel = new QPushButton(configPanel);
    pbCancel->setObjectName(QString::fromUtf8("pbCancel"));
    pbCancel->setText("Cancel");
    pbCancel->setGeometry(QRect(290, 160, 131, 23));
    pbWrite = new QPushButton(configPanel);
    pbWrite->setObjectName(QString::fromUtf8("pbWrite"));
    pbWrite->setText("Write");
    pbWrite->setToolTip("Write configures to device");
    pbWrite->setGeometry(QRect(430, 160, 131, 23));

    pbClose = new QPushButton(configPanel);
    pbClose->setObjectName(QString::fromUtf8("pbClose"));
    pbClose->setText("Close");
    pbClose->setToolTip("Close");
    pbClose->setGeometry(QRect(590, 160, 131, 23));

    // viewer pannel
    QWidget *viewerPanel = new QWidget(this);
    viewerPanel->setMinimumSize(QSize(600, 400));

    viewer1 = new UI_compassViewer(viewerPanel);
    viewer1->setObjectName(QString::fromUtf8("viewer1"));
    viewer1->setGeometry(QRect(10, 10, 371, 341));
    viewer2 = new UI_compassViewer(viewerPanel);
    viewer2->setObjectName(QString::fromUtf8("viewer2"));
    viewer2->setGeometry(QRect(390, 10, 371, 341));

    QHBoxLayout *hbLayout = new QHBoxLayout;
    hbLayout->setSpacing(4);
    hbLayout->addWidget(viewer1);
    hbLayout->addWidget(viewer2);
    viewerPanel->setLayout(hbLayout);

    // overall layout
    QVBoxLayout *vbLayout = new QVBoxLayout;
    vbLayout->addWidget(configPanel);
    vbLayout->addWidget(viewerPanel, 1);
    this->setLayout(vbLayout);

    // connect signals
    connect(pbBegin, SIGNAL(clicked(bool)),
            this, SLOT(btnBegin_clicked(bool)));
    connect(pbEnd, SIGNAL(clicked(bool)),
            this, SLOT(btnEnd_clicked(bool)));
    connect(pbCancel, SIGNAL(clicked(bool)),
            this, SLOT(btnCancel_clicked(bool)));
    connect(pbWrite, SIGNAL(clicked(bool)),
            this, SLOT(btnWrite_clicked(bool)));
    connect(pbClose, SIGNAL(clicked(bool)),
            this, SLOT(btnClose_clicked(bool)));

    // set oriention values
    cbOri1->addItem("None 0");
    cbOri1->addItem("Yaw 45");
    cbOri1->addItem("Yaw 90");
    cbOri1->addItem("Yaw 135");
    cbOri1->addItem("Yaw 180");
    cbOri1->addItem("Yaw 225");
    cbOri1->addItem("Yaw 270");
    cbOri1->addItem("Yaw 315");
    cbOri1->addItem("Roll 180");
    cbOri1->addItem("Roll 180, Yaw 45");
    cbOri1->addItem("Roll 180, Yaw 90");
    cbOri1->addItem("Roll 180, Yaw 135");
    cbOri1->addItem("Pitch 180");
    cbOri1->addItem("Roll 180, Yaw 225");
    cbOri1->addItem("Roll 180, Yaw 270");
    cbOri1->addItem("Roll 180, Yaw 315");
    cbOri1->addItem("Roll 90");
    cbOri1->addItem("Roll 90, Yaw 45");
    cbOri1->addItem("Roll 90, Yaw 90");
    cbOri1->addItem("Roll 90, Yaw 135");
    cbOri1->addItem("Roll 270");
    cbOri1->addItem("Roll 270, Yaw 45");
    cbOri1->addItem("Roll 270, Yaw 90");
    cbOri1->addItem("Roll 270, Yaw 135");
    cbOri1->addItem("Pitch 90");
    cbOri1->addItem("Pitch 270");

    cbOri2->addItem("None 0");
    cbOri2->addItem("Yaw 45");
    cbOri2->addItem("Yaw 90");
    cbOri2->addItem("Yaw 135");
    cbOri2->addItem("Yaw 180");
    cbOri2->addItem("Yaw 225");
    cbOri2->addItem("Yaw 270");
    cbOri2->addItem("Yaw 315");
    cbOri2->addItem("Roll 180");
    cbOri2->addItem("Roll 180, Yaw 45");
    cbOri2->addItem("Roll 180, Yaw 90");
    cbOri2->addItem("Roll 180, Yaw 135");
    cbOri2->addItem("Pitch 180");
    cbOri2->addItem("Roll 180, Yaw 225");
    cbOri2->addItem("Roll 180, Yaw 270");
    cbOri2->addItem("Roll 180, Yaw 315");
    cbOri2->addItem("Roll 90");
    cbOri2->addItem("Roll 90, Yaw 45");
    cbOri2->addItem("Roll 90, Yaw 90");
    cbOri2->addItem("Roll 90, Yaw 135");
    cbOri2->addItem("Roll 270");
    cbOri2->addItem("Roll 270, Yaw 45");
    cbOri2->addItem("Roll 270, Yaw 90");
    cbOri2->addItem("Roll 270, Yaw 135");
    cbOri2->addItem("Pitch 90");
    cbOri2->addItem("Pitch 270");
}

void UI_compassConfigControl::timerEvent(QTimerEvent *event)
{
    char    buf[256];

    sprintf(buf, "devID: %8d [%8.2f %8.2f %8.2f] -> [%8.2f %8.2f %8.2f] (N=%d)",
            m_devID1,
            m_off1.x, m_off1.y, m_off1.z,
            m_offNew1.x, m_offNew1.y, m_offNew1.z,
            m_compass1Data.size());
    leOri1->setText(QString::fromStdString(buf));

    sprintf(buf, "devID: %8d [%8.2f %8.2f %8.2f] -> [%8.2f %8.2f %8.2f] (N=%d)",
            m_devID2,
            m_off2.x, m_off2.y, m_off2.z,
            m_offNew2.x, m_offNew2.y, m_offNew2.z,
            m_compass2Data.size());
    leOri2->setText(QString::fromStdString(buf));
}

void UI_compassConfigControl::closeEvent(QCloseEvent *event)
{
    _close();
}

int UI_compassConfigControl::setUAS(UAS_Base *uas)
{
    m_uas = uas;
}

int UI_compassConfigControl::parseMavlinkMsg(mavlink_message_t *msg)
{
    static int Mx1 = 0, My1 = 0, Mz1 = 0,
               Mx2 = 0, My2 = 0, Mz2 = 0;
    int minDiff = 1;

    if( msg->msgid == MAVLINK_MSG_ID_RAW_IMU ) {
        mavlink_raw_imu_t msg_imu_raw;
        mavlink_msg_raw_imu_decode(msg, &msg_imu_raw);

        int Mx, My, Mz;

        Mx = msg_imu_raw.xmag;
        My = msg_imu_raw.ymag;
        Mz = msg_imu_raw.zmag;

        if( fabs(Mx-Mx1) > minDiff || fabs(My-My1) > minDiff || fabs(Mz-Mz1) > minDiff ) {
            dbg_pi("Compass1: Mx, My, Mz = %d, %d, %d\n", Mx, My, Mz);

            Point3d v(Mx, My, Mz);
            m_compass1Data.push_back(v);
            viewer1->addData(v);

            Mx1 = Mx;
            My1 = My;
            Mz1 = Mz;
        }
    }

    if( msg->msgid == MAVLINK_MSG_ID_SCALED_IMU2 ) {
        mavlink_scaled_imu2_t msg_imu_raw;
        mavlink_msg_scaled_imu2_decode(msg, &msg_imu_raw);

        int Mx, My, Mz;

        Mx = msg_imu_raw.xmag;
        My = msg_imu_raw.ymag;
        Mz = msg_imu_raw.zmag;

        if( fabs(Mx-Mx2) > minDiff || fabs(My-My2) > minDiff || fabs(Mz-Mz2) > minDiff ) {
            dbg_pi("Compass2: Mx, My, Mz = %d, %d, %d\n", Mx, My, Mz);

            Point3d v(Mx, My, Mz);
            m_compass2Data.push_back(v);
            viewer2->addData(v);

            Mx2 = Mx;
            My2 = My;
            Mz2 = Mz;
        }
    }
}

void UI_compassConfigControl::_begin(void)
{
    if( m_uas == NULL ) return;

    if( m_isCalibration ) return;

    // clear old data
    m_compass1Data.clear();
    m_compass2Data.clear();

    viewer1->clearData();
    viewer2->clearData();

    // get old offsets
    AP_ParamItem *pi;

    if( m_uas->m_paramArray.isLoaded() ) {
        pi = m_uas->m_paramArray.get("COMPASS_OFS_X");   if( pi != NULL ) m_off1.x = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS_Y");   if( pi != NULL ) m_off1.y = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS_Z");   if( pi != NULL ) m_off1.z = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS2_X");  if( pi != NULL ) m_off2.x = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS2_Y");  if( pi != NULL ) m_off2.y = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS2_Z");  if( pi != NULL ) m_off2.z = pi->toFloat();

        pi = m_uas->m_paramArray.get("COMPASS_DEV_ID");  if( pi != NULL ) m_devID1 = pi->toInt32();
        pi = m_uas->m_paramArray.get("COMPASS_DEV_ID2"); if( pi != NULL ) m_devID2 = pi->toInt32();
    } else {
        QMessageBox::warning(this,
                             tr("Parameter not load"),
                             tr("Please load parameters first!"));
        return;
    }

    // set compass oriention
    pi = m_uas->m_paramArray.get("COMPASS_ORIENT");
    if( pi != NULL ) cbOri1->setCurrentIndex(pi->toInt32());

    // set raw IMU data frequency
    old_streamRawSensorsFreq = m_uas->getStreamFrequency(MAV_DATA_STREAM_RAW_SENSORS);
    m_uas->setStreamFrequency(MAV_DATA_STREAM_RAW_SENSORS, 10);

    // regist mavlink message handle
    Mavlink_Message_Handle h = tr1::bind(&UI_compassConfigControl::parseMavlinkMsg,
                                         this, std::tr1::placeholders::_1);
    m_uas->registMavlinkMessageHandle("UI_compassConfigControl", h);

    m_isCalibration = 1;

}

void UI_compassConfigControl::_end(void)
{
    if( m_uas == NULL ) return;

    if( !m_isCalibration ) return;

    m_uas->setStreamFrequency(MAV_DATA_STREAM_RAW_SENSORS, old_streamRawSensorsFreq);
    m_uas->unregistMavlinkMessageHandle("UI_compassConfigControl");

    m_isCalibration = 0;

    // get all data
    dbg_pi("COMPASS 1 data numer: %d\n", m_compass1Data.size());
    dbg_pi("COMPASS 2 data numer: %d\n", m_compass2Data.size());

    if( m_compass1Data.size() < 10 ) {
        QMessageBox::warning(this,
                             tr("Compass Calibration Failed"),
                             tr("Not enough data points to calibrate the compass."));
        return;
    }

    // begin fitting sphere
    m_offNew1 = fittingSphere(m_compass1Data);
    if( m_compass2Data.size() > 10 ) m_offNew2 = fittingSphere(m_compass2Data);

    dbg_pi("COMPASS 1 off: %f %f %f\n", m_offNew1.x, m_offNew1.y, m_offNew1.z);
    dbg_pi("COMPASS 2 off: %f %f %f\n", m_offNew2.x, m_offNew2.y, m_offNew2.z);
}

void UI_compassConfigControl::_cancel(void)
{
    if( m_uas == NULL ) return;

    if( !m_isCalibration ) return;

    m_uas->setStreamFrequency(MAV_DATA_STREAM_RAW_SENSORS, old_streamRawSensorsFreq);
    m_uas->unregistMavlinkMessageHandle("UI_compassConfigControl");

    m_isCalibration = 0;
}

void UI_compassConfigControl::_write(void)
{
    if( m_uas == NULL ) return;

    if( m_isCalibration ) return;

    // get old values
    AP_ParamItem *pi;

    if( m_uas->m_paramArray.isLoaded() ) {
        pi = m_uas->m_paramArray.get("COMPASS_OFS_X");   if( pi != NULL ) m_off1.x = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS_Y");   if( pi != NULL ) m_off1.y = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS_Z");   if( pi != NULL ) m_off1.z = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS2_X");  if( pi != NULL ) m_off2.x = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS2_Y");  if( pi != NULL ) m_off2.y = pi->toFloat();
        pi = m_uas->m_paramArray.get("COMPASS_OFS2_Z");  if( pi != NULL ) m_off2.z = pi->toFloat();

        pi = m_uas->m_paramArray.get("COMPASS_DEV_ID");  if( pi != NULL ) m_devID1 = pi->toInt32();
        pi = m_uas->m_paramArray.get("COMPASS_DEV_ID2"); if( pi != NULL ) m_devID2 = pi->toInt32();
    } else {
        QMessageBox::warning(this,
                             tr("Parameter not load"),
                             tr("Please load parameters first!"));
        return;
    }

    // update values
    pi = m_uas->m_paramArray.get("COMPASS_OFS_X");   if( pi != NULL ) pi->fromFloat(m_offNew1.x + m_off1.x);
    pi = m_uas->m_paramArray.get("COMPASS_OFS_Y");   if( pi != NULL ) pi->fromFloat(m_offNew1.y + m_off1.y);
    pi = m_uas->m_paramArray.get("COMPASS_OFS_Z");   if( pi != NULL ) pi->fromFloat(m_offNew1.z + m_off1.z);
    pi = m_uas->m_paramArray.get("COMPASS_OFS2_X");  if( pi != NULL ) pi->fromFloat(m_offNew2.x + m_off2.x);
    pi = m_uas->m_paramArray.get("COMPASS_OFS2_Y");  if( pi != NULL ) pi->fromFloat(m_offNew2.y + m_off2.y);
    pi = m_uas->m_paramArray.get("COMPASS_OFS2_Z");  if( pi != NULL ) pi->fromFloat(m_offNew2.z + m_off2.z);

    pi = m_uas->m_paramArray.get("COMPASS_DEV_ID");  if( pi != NULL ) pi->fromInt32(m_devID1);
    pi = m_uas->m_paramArray.get("COMPASS_DEV_ID2"); if( pi != NULL ) pi->fromInt32(m_devID2);

    // get compass oriention
    int compOri = cbOri1->currentIndex();
    pi = m_uas->m_paramArray.get("COMPASS_ORIENT");
    if( pi != NULL ) pi->fromInt32(compOri);

    // upload to device
    StringArray sa;
    sa.push_back("COMPASS_OFS_X");   sa.push_back("COMPASS_OFS_Y");  sa.push_back("COMPASS_OFS_Z");
    sa.push_back("COMPASS_OFS2_X");  sa.push_back("COMPASS_OFS2_Y"); sa.push_back("COMPASS_OFS2_Z");
    sa.push_back("COMPASS_ORIENT");
    sa.push_back("COMPASS_DEV_ID");
    sa.push_back("COMPASS_DEV_ID2");
    m_uas->m_paramArray.updateParameters(sa);
}

void UI_compassConfigControl::_close(void)
{
    if( m_uas && m_isCalibration ) {
        m_uas->setStreamFrequency(MAV_DATA_STREAM_RAW_SENSORS, old_streamRawSensorsFreq);
        m_uas->unregistMavlinkMessageHandle("UI_compassConfigControl");

        m_isCalibration = 0;
    }
}


void UI_compassConfigControl::btnBegin_clicked(bool checked)
{
    _begin();
}

void UI_compassConfigControl::btnEnd_clicked(bool checked)
{
    _end();
}

void UI_compassConfigControl::btnCancel_clicked(bool checked)
{
    _cancel();
}

void UI_compassConfigControl::btnWrite_clicked(bool checked)
{
    _write();
}

void UI_compassConfigControl::btnClose_clicked(bool checked)
{
    _close();

    close();
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

UI_flightControlPanel::UI_flightControlPanel(QWidget *parent) :
    QWidget(parent)
{
    m_uasManager = NULL;

    m_currUAV = -1;
    m_oldUAV = -1;

    m_lastCMD = CMD_NONE;
    m_tmLastCmd = 0;

    setupUI();

    startTimer(100);
}

UI_flightControlPanel::~UI_flightControlPanel()
{

}

void UI_flightControlPanel::setupUI(void)
{
    this->setObjectName(QString::fromUtf8("FlightControlPanel"));
    this->setMinimumSize(223, 204);

    labUAS = new QLabel(this);
    labUAS->setObjectName(QString::fromUtf8("labUAS"));
    labUAS->setGeometry(QRect(10, 14, 81, 16));
    labUAS->setText("Active UAV:");
    cbUAS = new QComboBox(this);
    cbUAS->setObjectName(QString::fromUtf8("cbUAS"));
    cbUAS->setGeometry(QRect(96, 8, 153, 27));

    labFlightMode = new QLabel(this);
    labFlightMode->setObjectName(QString::fromUtf8("labFlightMode"));
    labFlightMode->setGeometry(QRect(10, 43, 81, 16));
    labFlightMode->setText("FlightMode:");
    cbFlightMode = new QComboBox(this);
    cbFlightMode->setObjectName(QString::fromUtf8("cbFlightMode"));
    cbFlightMode->setGeometry(QRect(96, 38, 153, 27));

    cbFlightMode->addItem("MANUAL");
    cbFlightMode->addItem("ALTCTL");
    cbFlightMode->addItem("POSCTL");
    cbFlightMode->addItem("AUTO_MISSION");
    cbFlightMode->addItem("AUTO_LOITER");
    cbFlightMode->addItem("AUTO_RTL");
    cbFlightMode->addItem("STABILIZED");
    cbFlightMode->addItem("OFFBOARD");
    cbFlightMode->addItem("AUTO_TAKEOFF");
    cbFlightMode->addItem("AUTO_LAND");


    edStatus = new QLineEdit(this);
    edStatus->setObjectName(QString::fromUtf8("edStatus"));
    edStatus->setGeometry(QRect(8, 68, 241, 25));
    edStatus->setReadOnly(true);


    btnArmDisarm = new QPushButton(this);
    btnArmDisarm->setObjectName(QString::fromUtf8("btnArmDisarm"));
    btnArmDisarm->setGeometry(QRect(6, 106, 120, 25));
    btnArmDisarm->setText("Arm/Disarm");
    btnRTL = new QPushButton(this);
    btnRTL->setObjectName(QString::fromUtf8("btnRTL"));
    btnRTL->setGeometry(QRect(130, 106, 120, 25));
    btnRTL->setText("RTL");

    btnTakeoff = new QPushButton(this);
    btnTakeoff->setObjectName(QString::fromUtf8("btnTakeoff"));
    btnTakeoff->setGeometry(QRect(6, 136, 120, 25));
    btnTakeoff->setText("Takeoff");
    btnLand = new QPushButton(this);
    btnLand->setObjectName(QString::fromUtf8("btnLand"));
    btnLand->setGeometry(QRect(130, 136, 120, 25));
    btnLand->setText("Land");


    btnAutoFlight = new QPushButton(this);
    btnAutoFlight->setObjectName(QString::fromUtf8("btnAutoFlight"));
    btnAutoFlight->setGeometry(QRect(6, 176, 244, 25));
    btnAutoFlight->setText("Auto Flight");


    btnStartMission = new QPushButton(this);
    btnStartMission->setObjectName(QString::fromUtf8("btnStartMission"));
    btnStartMission->setGeometry(QRect(6, 216, 120, 25));
    btnStartMission->setText("Start Mission");
    btnChangeSpeed = new QPushButton(this);
    btnChangeSpeed->setObjectName(QString::fromUtf8("btnChangeSpeed"));
    btnChangeSpeed->setGeometry(QRect(130, 216, 120, 25));
    btnChangeSpeed->setText("Change H-Speed");

    btnUploadMission = new QPushButton(this);
    btnUploadMission->setObjectName(QString::fromUtf8("btnUploadMission"));
    btnUploadMission->setGeometry(QRect(6, 246, 120, 25));
    btnUploadMission->setText("Upload Mission");
    btnDownloadMission = new QPushButton(this);
    btnDownloadMission->setObjectName(QString::fromUtf8("btnDownloadMission"));
    btnDownloadMission->setGeometry(QRect(130, 246, 120, 25));
    btnDownloadMission->setText("Download Mission");

    btnLandingGearDown = new QPushButton(this);
    btnLandingGearDown->setObjectName(QString::fromUtf8("btnLandingGearDown"));
    btnLandingGearDown->setGeometry(QRect(6, 276, 120, 25));
    btnLandingGearDown->setText("Landgear Down");
    btnLandingGearUp = new QPushButton(this);
    btnLandingGearUp->setObjectName(QString::fromUtf8("btnLandingGearUp"));
    btnLandingGearUp->setGeometry(QRect(130, 276, 120, 25));
    btnLandingGearUp->setText("Landgear Up");


    btnFollowMeStart = new QPushButton(this);
    btnFollowMeStart->setObjectName(QString::fromUtf8("btnFollowMeStart"));
    btnFollowMeStart->setGeometry(QRect(6, 316, 120, 25));
    btnFollowMeStart->setText("FollowMe On");
    btnFollowMeStop = new QPushButton(this);
    btnFollowMeStop->setObjectName(QString::fromUtf8("btnFollowMeStop"));
    btnFollowMeStop->setGeometry(QRect(130, 316, 120, 25));
    btnFollowMeStop->setText("FollowMe Off");


    // signals-slots
    connect(btnArmDisarm, SIGNAL(clicked(bool)),        this, SLOT(btnArmDisarm_clicked(bool)));
    connect(btnTakeoff, SIGNAL(clicked(bool)),          this, SLOT(btnTakeoff_clicked(bool)));
    connect(btnLand, SIGNAL(clicked(bool)),             this, SLOT(btnLand_clicked(bool)));
    connect(btnRTL, SIGNAL(clicked(bool)),              this, SLOT(btnRTL_clicked(bool)));

    connect(btnAutoFlight, SIGNAL(clicked(bool)),       this, SLOT(btnAutoFlight_clicked(bool)));
    connect(btnStartMission, SIGNAL(clicked(bool)),     this, SLOT(btnStartMission_clicked(bool)));
    connect(btnChangeSpeed, SIGNAL(clicked(bool)),      this, SLOT(btnChangeSpeed_clicked(bool)));
    connect(btnUploadMission, SIGNAL(clicked(bool)),    this, SLOT(btnUploadMission_clicked(bool)));
    connect(btnDownloadMission, SIGNAL(clicked(bool)),  this, SLOT(btnDownloadMission_clicked(bool)));
    connect(btnLandingGearDown, SIGNAL(clicked(bool)),  this, SLOT(btnLandingGearDown_clicked(bool)));
    connect(btnLandingGearUp, SIGNAL(clicked(bool)),    this, SLOT(btnLandingGearUp_clicked(bool)));

    connect(btnFollowMeStart, SIGNAL(clicked(bool)),    this, SLOT(btnFollowMeStart_clicked(bool)));
    connect(btnFollowMeStop, SIGNAL(clicked(bool)),     this, SLOT(btnFollowMeStop_clicked(bool)));


    connect(cbFlightMode, SIGNAL(currentIndexChanged(int)),
            this, SLOT(cbFlightMode_currentIndexChanged(int)));
    connect(cbUAS, SIGNAL(currentIndexChanged(int)),
            this, SLOT(cbUAS_currentIndexChanged(int)));
}

int UI_flightControlPanel::setUASManager(UAS_Manager *uasManager)
{
    m_uasManager = uasManager;
    updateUAVs();

    return 0;
}


int UI_flightControlPanel::updateUAVs(void)
{
    if( m_uasManager == NULL ) return -1;

    UAS_MAV *m = m_uasManager->get_active_mav();
    if( m != NULL ) {
        if( m_currUAV == -1 ) {
            m_currUAV = m->ID;
            m_oldUAV = m_currUAV;

            // regist message handle
            changeActiveUAV(-1, m_currUAV, 0);
        }
    } else {
        return -1;
    }

    // get UAV list
    UAS_Array ua;
    std::set<int> ua_set;
    int m_uasListModified = 0;

    m_uasManager->get_mav(ua);

    if( ua.size() == 0 ) m_uasListModified = 1;
    for(UAS_Array::iterator it=ua.begin(); it!=ua.end(); it++) {
        UAS_Base* u = *it;
        std::set<int>::iterator it2 = m_uavList.find(u->ID);
        if( it2 == m_uavList.end() ) {
            m_uasListModified = 1;
            m_uavList.insert(u->ID);
        }

        ua_set.insert(u->ID);
    }

    for(std::set<int>::iterator it=m_uavList.begin(); it!=m_uavList.end(); it++) {
        std::set<int>::iterator it2 = ua_set.find(*it);
        if( it2 == ua_set.end() ) m_uasListModified = 1;
    }

    // update UAV combobox
    if( m_uasListModified ) {
        int idxActive = 0;

        m_uavList.clear();
        for(UAS_Array::iterator it=ua.begin(); it!=ua.end(); it++) {
            UAS_Base* u = *it;
            m_uavList.insert(u->ID);
        }

        cbUAS->clear();

        if( m_uavList.size() > 0 ) {
            int idx = 0;
            for(std::set<int>::iterator it=m_uavList.begin(); it!=m_uavList.end(); it++) {
                int id = *it;
                QString un = QString("%1").arg(id);

                cbUAS->addItem(un);
                if( id == m_currUAV ) idxActive = idx;

                idx ++;
            }

            cbUAS->setCurrentIndex(idxActive);
        } else {
            cbUAS->setCurrentIndex(0);

            // unregist message handle
            changeActiveUAV(m_currUAV, -1, 0);

            m_currUAV = -1;
            m_oldUAV = -1;
        }
    }

    // update flight mode
    int fm = cbFlightMode->findText(QString::fromStdString(m->szFlightMode));
    if( fm != cbFlightMode->currentIndex() ) {
        cbFlightMode->setCurrentIndex(fm);
    }

    // update status
    {
        string st = trim(fmt::sprintf("%s, %s, %s, %s",
                                      m->szFlightMode, m->szMotorArmed,
                                      m->szSystemStatus,
                                      m->getAutoFlightStage()));

        edStatus->setText(QString::fromStdString(st));
    }
}

int UI_flightControlPanel::parseMavlinkMsg(mavlink_message_t *msg)
{
    if( msg->msgid == MAVLINK_MSG_ID_COMMAND_ACK ) {
        mavlink_command_ack_t pack;
        mavlink_msg_command_ack_decode(msg, &pack);

        string cmd = "UNKNOWN";
        switch(pack.command) {
        case MAV_CMD_NAV_TAKEOFF:
            cmd = "MAV_CMD_NAV_TAKEOFF";
            break;

        case MAV_CMD_NAV_LAND:
            cmd = "MAV_CMD_NAV_LAND";
            break;

        case MAV_CMD_COMPONENT_ARM_DISARM:
            cmd = "MAV_CMD_COMPONENT_ARM_DISARM";
            break;

        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
            cmd = "MAV_CMD_NAV_RETURN_TO_LAUNCH";
            break;

        case MAV_CMD_MISSION_START:
            cmd = "MAV_CMD_MISSION_START";
            break;

        case MAV_CMD_DO_CHANGE_SPEED:
            cmd = "MAV_CMD_DO_CHANGE_SPEED";
            break;

        case MAVLINK_MSG_ID_SET_MODE:
            cmd = "MAV_CMD_DO_CHANGE_SPEED";
            break;
        }

        dbg_pt("command confirm CMD: %s(%d), RESULT: %d",
               cmd.c_str(), pack.command,
               pack.result);

        if( pack.result == 0 ) {
            m_lastCMD = CMD_NONE;
            m_tmLastCmd = 0;
        }
    }

    return 0;
}

void UI_flightControlPanel::timerEvent(QTimerEvent *event)
{
    if( m_uasManager == NULL ) return;
    UAS_MAV *m = m_uasManager->get_active_mav();
    if( m == NULL ) return;

    double &timeout = svar.GetDouble("FastGCS.MAVLINK.timeoutExecCommand", 1.0);

    // check last command is executed
    if( m_lastCMD != CMD_NONE ) {
        double tmNow = tm_getTimeStamp();

        if( tmNow - m_tmLastCmd > timeout ) {
            QString msg;

            switch(m_lastCMD) {
            case CMD_CH_FLIGHTMODE:
                msg = QString("Failed to change FlightMode!");
                break;

            case CMD_LAND:
                if( m->customMode == MFM_AUTO_LAND ) {
                    m_lastCMD = CMD_NONE;
                    m_tmLastCmd = 0;
                    goto TIMEOUT_EXIT;
                } else {
                    msg = QString("Failed LAND!");
                }

                break;

            case CMD_ARM:
                if( m->motorArmed ) {
                    m_lastCMD = CMD_NONE;
                    m_tmLastCmd = 0;
                    goto TIMEOUT_EXIT;
                } else {
                    msg = QString("Failed ARM motor!");
                }

                break;

            case CMD_DISARM:
                if( !m->motorArmed ) {
                    m_lastCMD = CMD_NONE;
                    m_tmLastCmd = 0;
                    goto TIMEOUT_EXIT;
                } else {
                    msg = QString("Failed DISARM motor!");
                }

                break;

            case CMD_TAKEOFF:
                msg = QString("Failed TAKEOFF!");
                break;

            case CMD_RTL:
                if( m->customMode == MFM_AUTO_RTL ) {
                    m_lastCMD = CMD_NONE;
                    m_tmLastCmd = 0;
                    goto TIMEOUT_EXIT;
                } else {
                    msg = QString("Failed RTL!");
                }

                break;

            case CMD_STARTMISSION:
                if( m->customMode == MFM_AUTO_MISSION ) {
                    m_lastCMD = CMD_NONE;
                    m_tmLastCmd = 0;
                    goto TIMEOUT_EXIT;
                } else {
                    msg = QString("Failed Start Mission!");
                }

                break;

            case CMD_CHANGESPEED:
                msg = QString("Failed Change Speed!");
                break;
            }

            m_lastCMD = CMD_NONE;
            m_tmLastCmd = 0;

            if( 0 ) {
                QMessageBox msgbox(QMessageBox::Warning,
                                   "Warning", msg);
                msgbox.exec();
            } else {
                dbg_pe("%s", msg.toStdString().c_str());
            }
        }
    }

TIMEOUT_EXIT:
    // update UAV information
    updateUAVs();
}


void UI_flightControlPanel::btnArmDisarm_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    if( m->motorArmed ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to DISARM motors?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;

        // FIXME: better way
        if( m->gpH < 10 ) {
            m->executeCommand(MAV_CMD_COMPONENT_ARM_DISARM, 1,
                              0.0, 21196.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                              1);

            m_lastCMD = CMD_DISARM;
            m_tmLastCmd = tm_getTimeStamp();

            dbg_pi("system DISARM");
        } else {
            QMessageBox msgbox(QMessageBox::Warning,
                               "Warning", "Plese DISARM after landed!");
            msgbox.exec();
        }
    } else {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to ARM motors?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;

        m->executeCommand(MAV_CMD_COMPONENT_ARM_DISARM, 1,
                          1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                          1);

        m_lastCMD = CMD_ARM;
        m_tmLastCmd = tm_getTimeStamp();

        dbg_pi("system ARM");
    }
}

void UI_flightControlPanel::btnTakeoff_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    if( m->motorArmed ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to TAKEOFF?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;

        double &takeoff_alt = svar.GetDouble("MAVLINK.takeoff.Alt", 10);
        bool ok = true;
        double h = QInputDialog::getDouble(this,
                                             tr("Takeoff Altitude"),
                                             tr("Height (m):"),
                                             takeoff_alt, 4, 50,
                                             1,
                                             &ok);
        if ( ok ) {
            takeoff_alt = h;

            float must_navigate = 0;

            // must_navigate = is_zero(packet.param3)
            m->executeCommand(MAV_CMD_NAV_TAKEOFF, 1,
                              0.0, 0.0, must_navigate, 0.0, 0.0, 0.0, takeoff_alt,
                              1);

            m_lastCMD = CMD_TAKEOFF;
            m_tmLastCmd = tm_getTimeStamp();

            dbg_pi("begin TAKEOFF");
        }
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "Please DISARM first!");
        msgbox.exec();
    }
}

void UI_flightControlPanel::btnLand_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    if( m->motorArmed ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to LAND?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;


        // send command
        m->executeCommand(MAV_CMD_NAV_LAND, 1,
                          0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                          1);

        m_lastCMD = CMD_LAND;
        m_tmLastCmd = tm_getTimeStamp();

        dbg_pi("begin LAND");
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "UAV in ground!");
        msgbox.exec();
    }
}

void UI_flightControlPanel::btnRTL_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    if( m->motorArmed ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to RTL?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;


        // send command RTL
        m->executeCommand(MAV_CMD_NAV_RETURN_TO_LAUNCH, 1,
                          0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                          1);

        m_lastCMD = CMD_RTL;
        m_tmLastCmd = tm_getTimeStamp();

        dbg_pi("begin RTL");
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "UAV in ground!");
        msgbox.exec();
    }
}


void UI_flightControlPanel::btnAutoFlight_clicked(bool checked)
{
    // check current active MAV
    UAS_MAV *m = NULL;
    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    // confirm AutoFlight
    {
        QMessageBox msgbox(QMessageBox::Question,
                           "!!!Dangerous!!!",
                           "Mission is uploaded? Mission is correct?\n"
                           "Please check carefully!\nReally want AutoFlight?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;
    }

    // cancel AutoFlight
    if( m->getAutoFlightStage() != "AF_NONE" ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to Cancel AutoFlight?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;

        m->stopAutoFlight();

        return;
    }

    // perform AutoFlight
    dbg_pi("begin AutoFlight");
    int ret = m->doAutoFlight();

    if( ret == 0 )
        return;
    else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "System not meet autoFlight!");
        msgbox.exec();
    }

}

void UI_flightControlPanel::btnStartMission_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    if( m->motorArmed || 1 ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to START MISSION?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;


        // send command start mission
        m->executeCommand(MAV_CMD_MISSION_START, 1,
                          0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                          1);

        m_lastCMD = CMD_STARTMISSION;
        m_tmLastCmd = tm_getTimeStamp();

        dbg_pi("start mission");
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "UAV in ground!");
        msgbox.exec();
    }
}

void UI_flightControlPanel::btnChangeSpeed_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();

    if( m == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    if( m->motorArmed ) {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure to Change Speed?",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;

        double &wp_spd = svar.GetDouble("FastGCS.MAVLINK.WPNAV_SPEED", 10);
        bool ok = true;
        double s = QInputDialog::getDouble(this,
                                             tr("WPNAV Speed"),
                                             tr("Speed (m/s):"),
                                             wp_spd, 1, 20,
                                             1,
                                             &ok);
        if ( ok ) {
            wp_spd = s;

            // send command
            m->executeCommand(MAV_CMD_DO_CHANGE_SPEED, 1,
                              0.0, wp_spd, 0.0, 0.0, 0.0, 0.0, 0.0,
                              1);

            m_lastCMD = CMD_CHANGESPEED;
            m_tmLastCmd = tm_getTimeStamp();

            dbg_pi("change speed to: %f m/s", wp_spd);
        }
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "UAV in ground!");
        msgbox.exec();
    }
}



void UI_flightControlPanel::btnUploadMission_clicked(bool checked)
{
    if( SvarWithType<GCS_MainWindow*>::instance().exist("GCS_MainWindow.ptr") ) {
        GCS_MainWindow *mw = SvarWithType<GCS_MainWindow*>::instance()["GCS_MainWindow.ptr"];

        if( mw ) mw->action_uploadWaypoints();
    }
}

void UI_flightControlPanel::btnDownloadMission_clicked(bool checked)
{
    if( SvarWithType<GCS_MainWindow*>::instance().exist("GCS_MainWindow.ptr") ) {
        GCS_MainWindow *mw = SvarWithType<GCS_MainWindow*>::instance()["GCS_MainWindow.ptr"];

        if( mw ) mw->action_downloadWaypoints();
    }
}

void UI_flightControlPanel::btnLandingGearDown_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();
    if( m == NULL ) return;

    m->landingGearDown();
}

void UI_flightControlPanel::btnLandingGearUp_clicked(bool checked)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();
    if( m == NULL ) return;

    m->landingGearUp();
}



void UI_flightControlPanel::btnFollowMeStart_clicked(bool checked)
{
    if( m_uasManager == NULL ) return;
    UAS_MAV *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // close old connections
    if( SvarWithType<GPS_Reader*>::instance().exist("GPS_Reader.ptr") ) {
        if( SvarWithType<GPS_Reader*>::instance()["GPS_Reader.ptr"] != NULL ) {
            btnFollowMeStop_clicked();
        }
    }

    // get available UART devices
    QStringList devList, devList_ = getComDevices();

    // filter main UART device
    string mainUART = svar.GetString("FastGCS.UART.mainDEV", "");   // get main UART devName
    if( mainUART.size() > 0 ) {
        for(QStringList::iterator it=devList_.begin(); it!=devList_.end(); it++) {
            if( it->toStdString() != mainUART ) devList.append(*it);
        }
    } else {
        devList = devList_;
    }

    // get user selection
    bool ok = false;
    QString devName = QInputDialog::getItem(this,
                                            "Select mobileGPS device", "device:",
                                            devList,
                                            0, false,
                                            &ok);
    if( !ok ) return;

    // open UART device
    UART *uart = new UART;
    uart->baud_rate = svar.GetInt("FastGCS.followMe.baud", 115200);
    uart->port_name = devName.toStdString();

    if( uart->open() != 0 ) {
        delete uart;

        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "Cann't connect to mobileGPS!");
        msgbox.exec();

        return;
    }

    // begin mobile GPS reader
    GPS_Reader *gpsReader = new GPS_Reader;
    gpsReader->m_uart = uart;
    gpsReader->begin();
    SvarWithType<GPS_Reader*>::instance()["GPS_Reader.ptr"] = gpsReader;
    svar.GetDouble("IPC.followMe.tmLast", 0.0) = 0.0;

    // set default height & turn on GuidedControl
    if( !u->m_guidedControl.posSetted ) {
        double h;

        if( u->gpH < svar.GetDouble("FastGCS.followMe.minH", 5) )
            h = 5.0;
        else
            h = u->gpH;

        u->m_guidedControl.setPosition(u->gpLat, u->gpLon, h);
    }

    u->m_guidedControl.controlSource = UAS_MAV::GCS_FOLLOWME;
    u->m_guidedControl.turnOn();

    // set to show target
    MapWidget* mw = SvarWithType<MapWidget*>::instance()["FastGCS.ui.MapWidget"];
    mw->SetShowGuidedTarget(true);

    dbg_pi("FollowMe start!");
}

void UI_flightControlPanel::btnFollowMeStop_clicked(bool checked)
{
    if( m_uasManager == NULL ) return;
    UAS_MAV *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    if( SvarWithType<GPS_Reader*>::instance().exist("GPS_Reader.ptr") ) {
        GPS_Reader *gpsReader = SvarWithType<GPS_Reader*>::instance()["GPS_Reader.ptr"];

        if( gpsReader != NULL ) {
            SvarWithType<GPS_Reader*>::instance()["GPS_Reader.ptr"] = NULL;

            gpsReader->end();

            if( gpsReader->m_uart != NULL ) {
                gpsReader->m_uart->close();
                delete gpsReader->m_uart;
                gpsReader->m_uart = NULL;
            }

            delete gpsReader;

            // stop guided control
            u->m_guidedControl.controlSource = UAS_MAV::GCS_MOUSE;
            u->m_guidedControl.turnOff();

            dbg_pi("FollowMe stopped!");
        }
    }
}

void UI_flightControlPanel::cbFlightMode_currentIndexChanged(int index)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();
    if( m == NULL ) return;

    string fm = cbFlightMode->currentText().toStdString();
    if( fm != m->szFlightMode ) {
        uint32_t newFlightModeInt = mavlink_px4_custommode_getInt(fm.c_str());
        uint32_t newFlightMode = mavlink_px4_custommode_getID(fm.c_str());

        // confirm when new flight mode is Auto_mission, Land, RTL
        if( newFlightMode == MFM_AUTO_MISSION || newFlightMode == MFM_AUTO_LAND || newFlightMode == MFM_AUTO_RTL ) {
            QString s = QString("Are you sure to change flight mode to %1").arg(cbFlightMode->currentText());
            QMessageBox msgbox(QMessageBox::Question,
                               "Confirm", s,
                               QMessageBox::Yes | QMessageBox::No);
            int v = msgbox.exec();
            if( v != QMessageBox::Yes ) return;
        }

        // send change flight mode
        mavlink_message_t msg;
        mavlink_set_mode_t cmd;

        // FIXME: PX4 - `base_mode` need to be cleared
        cmd.base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
        cmd.custom_mode = newFlightModeInt;
        cmd.target_system = m->ID;

        mavlink_msg_set_mode_encode(m->gcsID, m->gcsCompID, &msg, &cmd);
        m->sendMavlinkMsg(&msg);

        m_lastCMD = CMD_CH_FLIGHTMODE;
        m_tmLastCmd = tm_getTimeStamp();

        // play message
        string text = fmt::sprintf("change flight mode to: %s", fm.c_str());
        dbg_pi(text.c_str());
        scommand.Call("MessageTTS", text);
    }
}

void UI_flightControlPanel::cbUAS_currentIndexChanged(int index)
{
    UAS_MAV *m = NULL;

    if( m_uasManager != NULL ) m = m_uasManager->get_active_mav();
    if( m == NULL ) return;

    string uavID = trim(cbFlightMode->currentText().toStdString());
    if( uavID.size() > 0 ) {
        int ID = str_to_int(uavID);
        if( ID <= 0 ) return;

        if( m_currUAV != -1 && m_currUAV != ID )
            changeActiveUAV(m_currUAV, ID, 1);
    }
}


int UI_flightControlPanel::changeActiveUAV(int oldUAV, int newUAV, int confirm)
{
    dbg_pi("change UAV[%d] -> UAV[%d], confirm = %d", oldUAV, newUAV, confirm);

    string text = fmt::sprintf("change to UAV %d", newUAV);
    scommand.Call("MessageTTS", text);

    if( m_uasManager == NULL ) return -1;

    // confirm change UAV
    if( confirm ) {
        QString s = QString("Are you sure to change UAV to [%1]").arg(newUAV);
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", s,
                           QMessageBox::Yes | QMessageBox::No);
        if( QMessageBox::Yes != msgbox.exec() ) {
            int idx = cbUAS->findText(QString("%1").arg(oldUAV));
            if( idx >= 0 ) cbUAS->setCurrentIndex(idx);

            return -1;
        }
    }

    // unregist old message handle
    UAS_MAV* u_old = (UAS_MAV*) m_uasManager->get_uas(oldUAV);
    if( u_old != NULL ) {
        u_old->unregistMavlinkMessageHandle("UI_flightControlPanel");
    }

    // change active uav to new select one
    UAS_MAV* u_new = m_uasManager->set_active_mav(newUAV);

    // regist mavlink message handle
    if( u_new != NULL ) {
        Mavlink_Message_Handle h = tr1::bind(&UI_flightControlPanel::parseMavlinkMsg,
                                             this, std::tr1::placeholders::_1);
        u_new->registMavlinkMessageHandle("UI_flightControlPanel", h);
    }
}
